def adjust_lin_velocity(self, v):
objects = self.laser_data['robots'] + self.laser_data['targets']
for o in objects:
la = LinAng()
la.setAllCoords(o.center.getAllCoords())
if 0 < la.linear < sp.min_distance_to_robot and abs(
la.angular) < 45:
print ">>>>> STOP <<<<<"
return 0.1
return v
def __init__(self):
#Raio obtido
self.obtainedRadiusToBeacon = 0.0
#Angulo Obtido em Relacao ao Beacon
self.obtainedAngleToBeacon = 0.0
#Angulo Obtido em Relacao ao Robo
self.obtainedAngleToRobot = 0.0
#Distancia obtida
self.obtainedDistanceToRobot = 0.0
#Distancia obtida ao alien
self.obtainedDistanceToAlien = 0.0
#angulo obtido ao alien
self.obtainedAngleToAlien = 0.0
#nova dimensao
self.obtainedDistanceFromBeaconToRobot = 0.0
#novo controle
self.circularCoefToBeacon = 0.0
#novo controle
self.propDistCoefToRobot = 0.0
#novo controle
self.linearCoefBeaconToRobot = 0.0
#novo controle
self.propAngularCoefToRobot = 0.0
#novo controle
self.linearCoefToBeacon = 0.0
#self
self.proximityCoefToRobot = 0.0
#self
self.actualTime = 0.0
#self
self.previousTime = 0.0
#self
self.approachRadius = 0.0
#localizacao x,y , com base no frame centralizado em mim
self.beacon = LinAng()
self.robot = LinAng()
self.alien = LinAng()
#substituida por linear velocity, angular_velocity
self.linearVelocity = sp.linear_velocity
self.angularVelocity = sp.angular_velocity
self.realLinearVelocity = 0.0
self.realAngularVelocity = 0.0
#velocidade tangencial/linear da roda direita e esquerda #m/s
self.rightLinearVelocity = 0.0
self.leftLinearVelocity = 0.0
#rotacao da roda direita e esquerda #r.p.s
self.rightWheelRotation = 0.0
self.leftWheelRotation = 0.0
#Beacon detectado?
self.hasBeacon = False
#Robo detectado?
self.hasRobot = False
#Alien detectado?
self.hasAlien = False
#Status
self.status = 0
class circum:
def __init__(self):
#Raio obtido
self.obtainedRadiusToBeacon = 0.0
#Angulo Obtido em Relacao ao Beacon
self.obtainedAngleToBeacon = 0.0
#Angulo Obtido em Relacao ao Robo
self.obtainedAngleToRobot = 0.0
#Distancia obtida
self.obtainedDistanceToRobot = 0.0
#Distancia obtida ao alien
self.obtainedDistanceToAlien = 0.0
#angulo obtido ao alien
self.obtainedAngleToAlien = 0.0
#nova dimensao
self.obtainedDistanceFromBeaconToRobot = 0.0
#novo controle
self.circularCoefToBeacon = 0.0
#novo controle
self.propDistCoefToRobot = 0.0
#novo controle
self.linearCoefBeaconToRobot = 0.0
#novo controle
self.propAngularCoefToRobot = 0.0
#novo controle
self.linearCoefToBeacon = 0.0
#self
self.proximityCoefToRobot = 0.0
#self
self.actualTime = 0.0
#self
self.previousTime = 0.0
#self
self.approachRadius = 0.0
#localizacao x,y , com base no frame centralizado em mim
self.beacon = LinAng()
self.robot = LinAng()
self.alien = LinAng()
#substituida por linear velocity, angular_velocity
self.linearVelocity = sp.linear_velocity
self.angularVelocity = sp.angular_velocity
self.realLinearVelocity = 0.0
self.realAngularVelocity = 0.0
#velocidade tangencial/linear da roda direita e esquerda #m/s
self.rightLinearVelocity = 0.0
self.leftLinearVelocity = 0.0
#rotacao da roda direita e esquerda #r.p.s
self.rightWheelRotation = 0.0
self.leftWheelRotation = 0.0
#Beacon detectado?
self.hasBeacon = False
#Robo detectado?
self.hasRobot = False
#Alien detectado?
self.hasAlien = False
#Status
self.status = 0
def setTime(self, time):
self.previousTime = self.actualTime
self.actualTime = time
def getPropAngularCoefToBeacon(self):
#sensor_angle, obtained_angle, desired_angle
#calculo o coeficiente proporcional angular em relacao do angulo ao Beacon
#envolve angulo do sensor, angulo desejado e angulo obtido
#retorna 0.0 quando angulo_obtido = angulo_desejado
#intervalo de retorno [-1.0, 1.0] *** plotar arquivo: "proportionalAngularToBeacon.py"
#valor fixo
if self.beacon.angular <= sp.min_ctrl_angle:
print "b.a <= min"
return -1.0
#apenas semantica
elif self.beacon.angular >= sp.max_ctrl_angle:
print "b.a >= max"
return 1.0
#entao, min_ctrl_angle < self.beacon.angular < max_ctrl_angle
else:
print "min < b.a < max"
return 2 * (self.beacon.angular - sp.min_ctrl_angle) / (
sp.max_ctrl_angle - sp.min_ctrl_angle) - 1
def getPropRadialCoefToBeacon(self):
#sensor_radius, obtained_radius, desired_radius
#calcula o valor do coeficiente com base no raio ao Beacon
#envolve raio do sensor, raio desejado e raio obtido
#retorna 0.0 quando raio_obtido = raio_desejado
#intervalo de retorno [-1.0, 1.0] *** plotar arquivo: "proportionalRadialToBeacon.py"
if self.beacon.linear >= sp.desired_radius:
#y.flat[high] = (2 * (x - dR)/(sR - dR) + 1) / 2
print "b.l >= rd"
return (2 * (self.beacon.linear - sp.desired_radius) /
(sp.sensor_cone_radius - sp.desired_radius) + 1) / 2
else:
print "b.l < rd"
return (self.beacon.linear - sp.desired_radius) / (
sp.desired_radius - sp.min_distance_to_robot)
#y.flat[low] = (x - dR)/(dR - mD)
def getCircularCoefToBeacon(self):
#calcula o coeficiente para circular o beacon
#quando ha o Beacon, retorna a media dos valores obtidos por getProportionalAngularToBeacon e getProportionalRadialToBeacon
if self.beacon.angular == 0.0:
return 0.0
return (self.getPropAngularCoefToBeacon() +
self.getPropRadialCoefToBeacon()) / 2.0
def getPropAngularCoefToRobot(self):
#calcula o valor do coeficiente em relacao ao angulo ao robo (evitar colisao)
#envolve o angulo obtido e o angulo do sensor
#retorna valor [0.0, 1.0] *** plotar arquivo "getPropAngularCoefToRobot.py"
angle_to_robot = (
self.obtainedAngleToRobot**2)**0.5 #apenas valores positivos
return (1 - angle_to_robot / (sp.sensor_cone_angle / 2.0))**2
def getPropDistCoefToRobot(self):
#calcula coeficiente linear relacao a posicao do robo
#apenas se a distancia obtida for menor do que a desejada
#envolve a distancia obtida e a desejada em relacao ao robot
#retorna valores no intervalo [0.0,1.0] *** plotar arquivo "getPropDistCoefToRobot"
if (self.obtainedDistanceToRobot < sp.desired_distance_to_robot):
return 1 - self.obtainedDistanceToRobot / sp.desired_distance_to_robot
else:
return 0.0
def getLinearCoefBeaconToRobot(self, beacon, robot):
#calcula o coeficiente linear em relacao a distancia do beacon ao robo detectado
#plotar arquivo getLinearCoefBeaconToRobot.py
#distancia do beacon ao robot
obtainedDistanceFromBeaconToRobot = ((beacon.x - robot.x)**2 +
(beacon.y - robot.y)**2)**0.5
#atualiza atributo
self.obtainedDistanceFromBeaconToRobot = self.H(
self.hasRobot and self.hasBeacon,
True) * obtainedDistanceFromBeaconToRobot
minObtainedDistance = sp.desired_radius - sp.min_distance_to_robot
maxObtainedDistance = sp.desired_radius + sp.min_distance_to_robot
if ((minObtainedDistance < obtainedDistanceFromBeaconToRobot)
and (obtainedDistanceFromBeaconToRobot < maxObtainedDistance)):
modularDistance = ((obtainedDistanceFromBeaconToRobot -
sp.desired_radius)**2)**0.5
return 1 - modularDistance / sp.min_distance_to_robot
else:
return 0.0
def getProximityCoefToRobot(self):
#calcula o coeficiente de proximidade em relacao ao robo mais proximo
#envolve a distancia obtida, distancia desejada e minima distancia entre robos
#retorna [-0.5,0.5] *** plotar arquivo "getProximityCoefToRobot.py"
#quando a distancia desejada for igual obtida, retorna 0
if self.obtainedDistanceToRobot > 2 * sp.desired_distance_to_robot:
return 0.5
if self.obtainedDistanceToRobot < sp.min_distance_to_robot:
return -0.5
#senao, calcule y = (x - dD) / (2 * (dD - mD))
return (self.obtainedDistanceToRobot - sp.desired_distance_to_robot
) / (2 *
(sp.desired_distance_to_robot - sp.min_distance_to_robot))
def H(self, value, reference):
#Heaviside function
if value >= reference:
return 1
else:
return 0
def printAll(self, num_id):
#print "[", num_id, "]:: Ci:", self.interferenceCoef, "Cc:", self.conversionCoef, "Co", self.orientationCoef, "Cp", self.proximityCoefToRobot, "Ca", self.forwardCoef
print "-------- P", num_id, " ---------"
print("Robot Real Velocities (vL,vA): %6.2f m/s, %6.2f rad/s " %
(self.realLinearVelocity, self.realAngularVelocity))
print("p3dx_mover velocities (vL,vA): %6.2f m/s, %6.2f rad/s " %
(self.linearVelocity, self.angularVelocity))
#print("Velocidade Tang Direita : %6.2f m/s" % (self.rightLinearVelocity))
#print("Rotacao Roda Direita : %6.2f rad/s" % (self.rightWheelRotation))
#print("Velocidade Tang Esquerda : %6.2f m/s" % (self.leftLinearVelocity))
#print("Rotacao Roda Esquerda : %6.2f rad/s" % (self.leftWheelRotation))
#print("Beacon x,y : %6.2f, %6.2f " % (self.beacon))
print("Beacon (Raio, Angulo): %6.2f, %6.2f " %
(self.obtainedRadiusToBeacon, self.obtainedAngleToBeacon))
#print("Raio do Sensor (Sr): %6.2f" % (sp.sensor_cone_radius))
#print("Raio desejado (dR): %6.2f" % (sp.desired_radius))
#print("Circular Coef Beacon aCtB: %6.2f" % (self.circularCoefToBeacon))
#print("Robot x,y : %6.2f, %6.2f " % (self.robot))
print(" Robot (Dist, Angulo) : %6.2f, %6.2f" %
(self.obtainedDistanceToRobot, self.obtainedAngleToRobot))
#print("Min Distancia entre Rob(mD): %6.2f" % (sp.min_distance_to_robot))
#print("Distancia desejada (dD): %6.2f" % (sp.desired_distance_to_robot))
#print("proximityCoefToRobot : %6.2f " % (self.proximityCoefToRobot))
#print("propAngularCoefToRobot : %6.2f" % (self.propAngularCoefToRobot))
#print("propDistCoefToRobot : %6.2f" % (self.propDistCoefToRobot))
#print("Distancia Beacon to Robot : %6.2f" % (self.obtainedDistanceFromBeaconToRobot))
#print("Ang. Coef Beacon to Robot : %6.2f" % (self.linearCoefBeaconToRobot))
print sp.status_msg[self.status], self.status
print ""
print ""
#atualiza a existencia de objetos detectados
def updateDetectedObjects(self, detectedBeaconDist, detectedRobotDist,
detectedAlienDist):
if detectedBeaconDist.linear > 0.0:
self.hasBeacon = True
self.obtainedRadiusToBeacon, self.obtainedAngleToBeacon = detectedBeaconDist.getPolarCoords(
)
self.beacon.setRetCoords(detectedBeaconDist.getRetCoords())
self.beacon.setPolarCoords(detectedBeaconDist.getPolarCoords())
else:
self.hasBeacon = False
self.obtainedRadiusToBeacon, self.obtainedAngleToBeacon = 0.0, 0.0
self.beacon.clear()
if detectedRobotDist.linear > 0.0:
self.hasRobot = True
self.obtainedDistanceToRobot, self.obtainedAngleToRobot = detectedRobotDist.getPolarCoords(
)
self.robot.setRetCoords(detectedRobotDist.getRetCoords())
self.robot.setPolarCoords(detectedBeaconDist.getPolarCoords())
else:
self.hasRobot = False
self.obtainedDistanceToRobot, self.obtainedAngleToRobot = 0.0, 0.0
self.robot.clear()
if detectedAlienDist.linear > 0.0:
self.hasAlien = True
self.obtainedDistanceToAlien, self.obtainedAngleToAlien = detectedAlienDist.getPolarCoords(
)
self.alien.setRetCoords(detectedAlienDist.getRetCoords())
self.alien.setPolarCoords(detectedBeaconDist.getPolarCoords())
else:
self.hasAlien = False
self.obtainedDistanceToAlien, self.obtainedAngleToAlien = 0.0, 0.0
self.alien.clear()
def getDelimitedLinearVelocity(self, linearVelocity):
if (linearVelocity < 0.0):
return 0.0
elif (linearVelocity > sp.max_linear_velocity):
return sp.max_linear_velocity
else:
return linearVelocity
def getHardLimited(self, min_value, value, max_value):
hlim = 0.0
if (value < min_value):
hlim = min_value
elif (value > max_value):
hlim = max_value
else:
hlim = value
return hlim
def verifyMinDistances(self, numRobot, velocity):
stop = False
#prevents beacon collision
if (0 < self.obtainedRadiusToBeacon) and (self.obtainedRadiusToBeacon <
sp.min_distance_to_robot):
if ((self.obtainedAngleToBeacon**2)**0.5 < 45):
stop = True
elif (0 < self.obtainedDistanceToRobot) and (
self.obtainedDistanceToRobot < sp.min_distance_to_robot):
if ((self.obtainedAngleToRobot**2)**0.5 < 45):
stop = True
if stop == True:
#print "XXXXXXX Stopping Linear Velocity in P", numRobot, "XXXXXXX"
velocity = 0.0
return velocity
def getTransitionCoefs(self):
#utiliza o raio obter a transicao linear
#circRadiusCoef = 1.0
#appRadiusCoef = 0.0
#if self.beacon.linear >= sp.desired_radius:
# appRadiusCoef = (self.beacon.linear - sp.desired_radius)/(sp.sensor_cone_radius - sp.desired_radius)
# appRadiusCoef = (appRadiusCoef**2)**0.5
# circRadiusCoef = 1 - appRadiusCoef
#coeficiente do raio de aproximacao obtido de uma sigmoidal centrada no raio desejado
appRadiusCoef = self.getBotzmann(self.beacon.linear, sp.desired_radius,
sp.boltzmann_time_constant)
circRadiusCoef = 1 - appRadiusCoef
return circRadiusCoef, appRadiusCoef
def getBotzmann(self, linear_x, middle_x, const_time):
#calcula a funcao botzman com ponto central e tempo constante sobre x
return 1 / (1 + np.exp(-(linear_x - middle_x) / const_time))
#devolve as velocidades linear e angular
def getVelocities(self, numRobot, myVelocities, beaconCoord, robotCoord,
alienCoord, now):
#print "beacon coord", beaconCoord.getVelocities()
#print "robot coord", robotCoord.getVelocities()
#print "alien coord", alienCoord.getVelocities()
self.setTime(now.secs + now.nsecs / (10**9)) # segundos.milisegundos
#print ("Tempo decorrido %4.9f" %(self.actualTime - self.previousTime))
#atualiza a velocidade real
self.realLinearVelocity, self.realAngularVelocity = myVelocities.getVelocities(
)
#atualiza a existencia dos objetos
self.updateDetectedObjects(beaconCoord, robotCoord, alienCoord)
#age conforme os objetos detectados
if (self.hasBeacon and self.hasRobot): #Escorting
self.status = 3
#--- bloco comentado --- 2016.02.05
#self.circularCoefToBeacon = self.getCircularCoefToBeacon()
self.propAngularCoefToRobot = self.getPropAngularCoefToRobot()
self.proximityCoefToRobot = self.getProximityCoefToRobot(
) # * self.getPropAngularCoefToRobot()
#self.linearCoefBeaconToRobot = self.getLinearCoefBeaconToRobot(beaconCoord, robotCoord)
#-comentado anteriormente-#self.linearVelocity = sp.linear_velocity + sp.linear_velocity * self.linearCoefBeaconToRobot * self.proximityCoefToRobot
self.linearVelocity = sp.linear_velocity + sp.linear_velocity * self.propAngularCoefToRobot * self.proximityCoefToRobot
##mantenha o raio desejado
#self.angularVelocity = self.linearVelocity / sp.desired_radius + self.circularCoefToBeacon
#---fim do bloco comentado ---
elif (self.hasBeacon and not self.hasRobot): #Circulating
self.status = 2
circRadiusCoef, appRadiusCoef = self.getTransitionCoefs()
kpA = self.getPropAngularCoefToBeacon()
error_radius = sp.desired_radius - self.beacon.linear
appRadius = -(self.beacon.x**2 + self.beacon.y**2 -
sp.desired_radius**2) / (
2 * (sp.desired_radius - self.beacon.y))
circRadius = sp.desired_radius + error_radius
radius = circRadiusCoef * circRadius + appRadiusCoef * appRadius
ang_vel = self.linearVelocity / radius
hl_ang_vel = self.getHardLimited(
sp.min_angular_velocity, ang_vel,
sp.max_angular_velocity) + kpA * sp.delta_angular
self.angularVelocity = self.getHardLimited(sp.min_angular_velocity,
hl_ang_vel,
sp.max_angular_velocity)
log.info(str(now))
print "kpA", kpA, "error_radius", error_radius
print "appRadius", appRadius, "appRadiusCoef", appRadiusCoef, "=", appRadius * appRadiusCoef
print "circRadius", circRadius, "circRadiusCoef", circRadiusCoef, "=", circRadius * circRadiusCoef
print "radius", radius, "ang_vel", ang_vel, "hl_ang_vel", hl_ang_vel, "angularVelocity", self.angularVelocity
self.approachRadius = appRadius
self.beacon.prn()
elif (not self.hasBeacon and self.hasRobot): #Avoiding
self.status = 1
#self.proximityCoefToRobot = self.getProximityCoefToRobot()
#self.propAngularCoefToRobot = self.getPropAngularCoefToRobot()
#self.propDistCoefToRobot = self.getPropDistCoefToRobot()
#self.linearVelocity = sp.linear_velocity + sp.linear_velocity * self.proximityCoefToRobot * self.propAngularCoefToRobot
#self.angularVelocity = self.linearVelocity / self.obtainedDistanceToRobot + sp.angular_velocity * self.proximityCoefToRobot * self.propDistCoefToRobot
else: #status == "Seeking"
self.status = 0
self.linearVelocity = sp.linear_velocity
#self.angularVelocity = sp.angular_velocity + sp.angular_velocity * np.random.random() * 0.1
decimal_random_rate = np.random.random() / 10.0
self.angularVelocity = sp.angular_velocity * (1 +
decimal_random_rate)
### delimiter ###
self.linearVelocity = self.getDelimitedLinearVelocity(
self.linearVelocity)
#self.angularVelocity = self.getDelimitedAngularVelocity(self.angularVelocity)
### Parada de Seguranca ###
self.linearVelocity = self.verifyMinDistances(numRobot,
self.linearVelocity)
###------------------------------###
### Atualizacao das propriedades ###
myVelocities.angular = self.angularVelocity
myVelocities.linear = self.linearVelocity
#atualiza rotacoes e velocidades tangenciais #monitoramento
self.rightLinearVelocity = myVelocities.angular + myVelocities.linear
self.leftLinearVelocity = -2 * myVelocities.linear - myVelocities.angular
self.rightWheelRotation = self.rightLinearVelocity / (
sp.wheel_diameter * np.pi)
self.leftWheelRotation = self.leftLinearVelocity / (sp.wheel_diameter *
np.pi)
#self.printAll(numRobot)
return myVelocities
class circum:
def __init__(self):
#Raio obtido
self.obtainedRadiusToBeacon = 0.0
#Angulo Obtido em Relacao ao Beacon
self.obtainedAngleToBeacon = 0.0
#Angulo Obtido em Relacao ao Robo
self.obtainedAngleToRobot = 0.0
#Distancia obtida
self.obtainedDistanceToRobot = 0.0
#Distancia obtida ao alien
self.obtainedDistanceToAlien = 0.0
#angulo obtido ao alien
self.obtainedAngleToAlien = 0.0
#nova dimensao
self.obtainedDistanceFromBeaconToRobot = 0.0
#novo controle
self.circularCoefToBeacon = 0.0
#novo controle
self.propDistCoefToRobot = 0.0
#novo controle
self.linearCoefBeaconToRobot = 0.0
#novo controle
self.propAngularCoefToRobot = 0.0
#novo controle
self.linearCoefToBeacon = 0.0
#self
self.proximityCoefToRobot = 0.0
#self
self.actualTime = 0.0
#self
self.previousTime = 0.0
#self
self.approachRadius = 0.0
#localizacao x,y , com base no frame centralizado em mim
self.beacon = LinAng()
self.robot = LinAng()
self.alien = LinAng()
#substituida por linear velocity, angular_velocity
self.linearVelocity = sp.linear_velocity
self.angularVelocity = sp.angular_velocity
self.realLinearVelocity = 0.0
self.realAngularVelocity = 0.0
#velocidade tangencial/linear da roda direita e esquerda #m/s
self.rightLinearVelocity = 0.0
self.leftLinearVelocity = 0.0
#rotacao da roda direita e esquerda #r.p.s
self.rightWheelRotation = 0.0
self.leftWheelRotation = 0.0
#Beacon detectado?
self.hasBeacon = False
#Robo detectado?
self.hasRobot = False
#Alien detectado?
self.hasAlien = False
#Status
self.status = 0
def setTime(self, time):
self.previousTime = self.actualTime
self.actualTime = time
def getApproachRadius(self):
#retorna o raio de abordagem, para realizacao de um arco tangente
radius = 0.0
#self.beacon.prn()
#lidando com a descontinuidade
# quando y do beacon for o inverso do raio desejado
if (self.beacon.y == sp.desired_radius):
if (self.beacon.angular == 90):
radius = sp.desired_radius
else:
radius = 0.0
else:
if (self.beacon.linear > sp.desired_radius) and (self.beacon.angular < 90):
#print self.actualTime, "beacon.linear <= sp.desired_radius and self.beacon.angular < 90"
radius = -(self.beacon.x**2 + self.beacon.y**2 - sp.desired_radius**2) / (2 * (sp.desired_radius - self.beacon.y))
#utilizando o modulo de y
#radius = -(self.beacon.x**2 + self.beacon.y**2 - sp.desired_radius**2) / (2 * (sp.desired_radius - (self.beacon.y**2)**0.5))
else:
if(self.beacon.angular < sp.desired_angle_to_beacon):
fator = (self.obtainedAngleToBeacon + sp.desired_angle_to_beacon)/(2 * sp.desired_angle_to_beacon)
#print self.actualTime, "self.beacon.angular < sp.desired_angle_to_beacon"
radius = self.linearVelocity / (sp.min_angular_velocity * fator)
else:
radius = ((self.beacon.x + sp.desired_radius)**2 + self.beacon.y**2)/(2 * self.beacon.y)
#print self.actualTime, "self.beacon.angular >= sp.desired_angle_to_beacon"
#print self.actualTime, "beacon.linear > sp.desired_radius"
#print "fi = ", 90 - self.beacon.angular
#fi = (90 - self.beacon.angular) * np.pi/180
#m = (self.beacon.linear + sp.desired_radius) * np.sin(fi)
#n = (self.beacon.linear + sp.desired_radius) * np.cos(fi)
#print "m: ", m, " n:", n
#radius = (n**2 + m**2)/(2*n)
print("%3.9f | %3.2f | %3.9f" % (self.beacon.linear, self.beacon.angular, radius))
#se o raio for maior do que o permitido
#if radius**2 > sp.max_approach_radius**2:
#radius = 0.0
return radius
def getPropAngularCoefToBeacon(self):
#sensor_angle, obtained_angle, desired_angle
#calculo o coeficiente proporcional angular em relacao do angulo ao Beacon
#envolve angulo do sensor, angulo desejado e angulo obtido
#retorna 0.0 quando angulo_obtido = angulo_desejado
#intervalo de retorno [-1.0, 1.0] *** plotar arquivo: "proportionalAngularToBeacon.py"
min_angle = 2 * sp.desired_angle_to_beacon - sp.sensor_cone_angle / 2
max_angle = sp.sensor_cone_angle / 2
if self.obtainedAngleToBeacon < min_angle:
return -1.0
elif self.obtainedAngleToBeacon > max_angle:
return 1.0
else:
return 2 * (self.obtainedAngleToBeacon - min_angle) / (max_angle - min_angle) - 1
def getPropRadialCoefToBeacon(self):
#sensor_radius, obtained_radius, desired_radius
#calcula o valor do coeficiente com base no raio ao Beacon
#envolve raio do sensor, raio desejado e raio obtido
#retorna 0.0 quando raio_obtido = raio_desejado
#intervalo de retorno [-1.0, 1.0] *** plotar arquivo: "proportionalRadialToBeacon.py"
return 2 * (self.obtainedRadiusToBeacon - sp.desired_radius) / sp.sensor_cone_radius
def getCircularCoefToBeacon(self):
#calcula o coeficiente para circular o beacon
#quando ha o Beacon, retorna a media dos valores obtidos por getProportionalAngularToBeacon e getProportionalRadialToBeacon
if self.obtainedRadiusToBeacon == 0.0:
return 0.0
return (self.getPropAngularCoefToBeacon() + self.getPropRadialCoefToBeacon())/2.0
def getPropAngularCoefToRobot(self):
#calcula o valor do coeficiente em relacao ao angulo ao robo (evitar colisao)
#envolve o angulo obtido e o angulo do sensor
#retorna valor [0.0, 1.0] *** plotar arquivo "getPropAngularCoefToRobot.py"
angle_to_robot = (self.obtainedAngleToRobot**2)**0.5 #apenas valores positivos
return (1 - angle_to_robot / (sp.sensor_cone_angle / 2.0))**2
def getPropDistCoefToRobot(self):
#calcula coeficiente linear relacao a posicao do robo
#apenas se a distancia obtida for menor do que a desejada
#envolve a distancia obtida e a desejada em relacao ao robot
#retorna valores no intervalo [0.0,1.0] *** plotar arquivo "getPropDistCoefToRobot"
if(self.obtainedDistanceToRobot < sp.desired_distance_to_robot):
return 1 - self.obtainedDistanceToRobot / sp.desired_distance_to_robot
else:
return 0.0
def getLinearCoefBeaconToRobot(self, beacon, robot):
#calcula o coeficiente linear em relacao a distancia do beacon ao robo detectado
#plotar arquivo getLinearCoefBeaconToRobot.py
#distancia do beacon ao robot
obtainedDistanceFromBeaconToRobot = ((beacon.x - robot.x)**2 + (beacon.y - robot.y)**2)**0.5
#atualiza atributo
self.obtainedDistanceFromBeaconToRobot = self.H(self.hasRobot and self.hasBeacon, True) * obtainedDistanceFromBeaconToRobot
minObtainedDistance = sp.desired_radius - sp.min_distance_to_robot
maxObtainedDistance = sp.desired_radius + sp.min_distance_to_robot
if ((minObtainedDistance < obtainedDistanceFromBeaconToRobot) and (obtainedDistanceFromBeaconToRobot < maxObtainedDistance )):
modularDistance = ((obtainedDistanceFromBeaconToRobot - sp.desired_radius)**2)**0.5
return 1 - modularDistance / sp.min_distance_to_robot
else:
return 0.0
def getProximityCoefToRobot(self):
#calcula o coeficiente de proximidade em relacao ao robo mais proximo
#envolve a distancia obtida, distancia desejada e minima distancia entre robos
#retorna [-0.5,0.5] *** plotar arquivo "getProximityCoefToRobot.py"
#quando a distancia desejada for igual obtida, retorna 0
if self.obtainedDistanceToRobot > 2 * sp.desired_distance_to_robot:
return 0.5
if self.obtainedDistanceToRobot < sp.min_distance_to_robot:
return -0.5
#senao, calcule y = (x - dD) / (2 * (dD - mD))
return (self.obtainedDistanceToRobot - sp.desired_distance_to_robot) / (2 * (sp.desired_distance_to_robot - sp.min_distance_to_robot))
def H(self, value, reference):
#Heaviside function
if value >= reference:
return 1
else:
return 0
def printAll(self, num_id):
#print "[", num_id, "]:: Ci:", self.interferenceCoef, "Cc:", self.conversionCoef, "Co", self.orientationCoef, "Cp", self.proximityCoefToRobot, "Ca", self.forwardCoef
print "-------- P", num_id, " ---------"
print("Robot Real Velocities (vL,vA): %6.2f m/s, %6.2f rad/s " % (self.realLinearVelocity,self.realAngularVelocity))
print("p3dx_mover velocities (vL,vA): %6.2f m/s, %6.2f rad/s " % (self.linearVelocity,self.angularVelocity))
#print("Velocidade Tang Direita : %6.2f m/s" % (self.rightLinearVelocity))
#print("Rotacao Roda Direita : %6.2f rad/s" % (self.rightWheelRotation))
#print("Velocidade Tang Esquerda : %6.2f m/s" % (self.leftLinearVelocity))
#print("Rotacao Roda Esquerda : %6.2f rad/s" % (self.leftWheelRotation))
#print("Beacon x,y : %6.2f, %6.2f " % (self.beacon))
print( "Beacon (Raio, Angulo): %6.2f, %6.2f " % (self.obtainedRadiusToBeacon, self.obtainedAngleToBeacon))
#print("Raio do Sensor (Sr): %6.2f" % (sp.sensor_cone_radius))
#print("Raio desejado (dR): %6.2f" % (sp.desired_radius))
#print("Circular Coef Beacon aCtB: %6.2f" % (self.circularCoefToBeacon))
#print("Robot x,y : %6.2f, %6.2f " % (self.robot))
print(" Robot (Dist, Angulo) : %6.2f, %6.2f" % (self.obtainedDistanceToRobot, self.obtainedAngleToRobot))
#print("Min Distancia entre Rob(mD): %6.2f" % (sp.min_distance_to_robot))
#print("Distancia desejada (dD): %6.2f" % (sp.desired_distance_to_robot))
#print("proximityCoefToRobot : %6.2f " % (self.proximityCoefToRobot))
#print("propAngularCoefToRobot : %6.2f" % (self.propAngularCoefToRobot))
#print("propDistCoefToRobot : %6.2f" % (self.propDistCoefToRobot))
#print("Distancia Beacon to Robot : %6.2f" % (self.obtainedDistanceFromBeaconToRobot))
#print("Ang. Coef Beacon to Robot : %6.2f" % (self.linearCoefBeaconToRobot))
print sp.status_msg[self.status], self.status
print ""
print ""
#atualiza a existencia de objetos detectados
def updateDetectedObjects(self, detectedBeaconDist, detectedRobotDist, detectedAlienDist):
if detectedBeaconDist.linear > 0.0:
self.hasBeacon = True
self.obtainedRadiusToBeacon, self.obtainedAngleToBeacon = detectedBeaconDist.getPolarCoords()
self.beacon.setRetCoords(detectedBeaconDist.getRetCoords())
self.beacon.setPolarCoords(detectedBeaconDist.getPolarCoords())
else:
self.hasBeacon = False
self.obtainedRadiusToBeacon, self.obtainedAngleToBeacon = 0.0, 0.0
self.beacon.clear()
if detectedRobotDist.linear > 0.0:
self.hasRobot = True
self.obtainedDistanceToRobot, self.obtainedAngleToRobot = detectedRobotDist.getPolarCoords()
self.robot.setRetCoords(detectedRobotDist.getRetCoords())
self.robot.setPolarCoords(detectedBeaconDist.getPolarCoords())
else:
self.hasRobot = False
self.obtainedDistanceToRobot, self.obtainedAngleToRobot = 0.0, 0.0
self.robot.clear()
if detectedAlienDist.linear > 0.0:
self.hasAlien = True
self.obtainedDistanceToAlien, self.obtainedAngleToAlien = detectedAlienDist.getPolarCoords()
self.alien.setRetCoords(detectedAlienDist.getRetCoords())
self.alien.setPolarCoords(detectedBeaconDist.getPolarCoords())
else:
self.hasAlien = False
self.obtainedDistanceToAlien, self.obtainedAngleToAlien = 0.0, 0.0
self.alien.clear()
def getDelimitedLinearVelocity(self, linearVelocity):
if(linearVelocity < 0.0):
return 0.0
elif(linearVelocity > sp.max_linear_velocity):
return sp.max_linear_velocity
else:
return linearVelocity
def getDelimitedAngularVelocity(self, angularVelocity):
if(angularVelocity < sp.min_angular_velocity):
return sp.min_angular_velocity
elif(angularVelocity > sp.max_angular_velocity):
return sp.max_angular_velocity
else:
return angularVelocity
def verifyMinDistances(self, numRobot, velocity):
stop = False
#prevents beacon collision
if(0 < self.obtainedRadiusToBeacon) and (self.obtainedRadiusToBeacon < sp.min_distance_to_robot):
if((self.obtainedAngleToBeacon**2)**0.5 < 45):
stop = True
elif(0 < self.obtainedDistanceToRobot) and (self.obtainedDistanceToRobot < sp.min_distance_to_robot):
if((self.obtainedAngleToRobot**2)**0.5 < 45):
stop = True
if stop == True:
#print "XXXXXXX Stopping Linear Velocity in P", numRobot, "XXXXXXX"
velocity = 0.0
return velocity
#devolve as velocidades linear e angular
def getVelocities(self, numRobot , myVelocities, beaconCoord, robotCoord, alienCoord, now):
#print "beacon coord", beaconCoord.getVelocities()
#print "robot coord", robotCoord.getVelocities()
#print "alien coord", alienCoord.getVelocities()
self.setTime(now.secs + now.nsecs / (10**9)) # segundos.milisegundos
#print ("Tempo decorrido %4.9f" %(self.actualTime - self.previousTime))
#atualiza a velocidade real
self.realLinearVelocity, self.realAngularVelocity = myVelocities.getVelocities()
#atualiza a existencia dos objetos
self.updateDetectedObjects(beaconCoord, robotCoord, alienCoord)
#age conforme os objetos detectados
if(self.hasBeacon and self.hasRobot): #Escorting
self.status = 3
self.circularCoefToBeacon = self.getCircularCoefToBeacon()
self.propAngularCoefToRobot = self.getPropAngularCoefToRobot()
self.proximityCoefToRobot = self.getProximityCoefToRobot() # * self.getPropAngularCoefToRobot()
self.linearCoefBeaconToRobot = self.getLinearCoefBeaconToRobot(beaconCoord, robotCoord)
#self.linearVelocity = sp.linear_velocity + sp.linear_velocity * self.linearCoefBeaconToRobot * self.proximityCoefToRobot
self.linearVelocity = sp.linear_velocity + sp.linear_velocity * self.propAngularCoefToRobot * self.proximityCoefToRobot
#mantenha o raio desejado
self.angularVelocity = self.linearVelocity / sp.desired_radius + self.circularCoefToBeacon
elif(self.hasBeacon and not self.hasRobot): #Circulating
self.status = 2
self.circularCoefToBeacon = self.getCircularCoefToBeacon()
self.propRadialCoefToBeacon = self.getPropRadialCoefToBeacon()
self.linearVelocity = sp.linear_velocity # / (1 + self.propRadialCoefToBeacon)# + (sp.max_linear_velocity - sp.min_linear_velocity) * self.circularCoefToBeacon
self.approachRadius = self.getApproachRadius()
if self.approachRadius == 0.0:
self.angularVelocity = 0.0
else:
#if(self.obtainedAngleToBeacon > 0.8 * sp.desired_angle_to_beacon and self.obtainedAngleToBeacon < 1.2 * sp.desired_angle_to_beacon):
#self.angularVelocity = self.linearVelocity / sp.desired_radius + self.circularCoefToBeacon
#else:
self.angularVelocity = self.linearVelocity / self.approachRadius
if self.angularVelocity > sp.max_angular_velocity:
#print "*** clipping on MAX angular velocity ***"
self.angularVelocity = sp.max_angular_velocity
self.linearVelocity = self.approachRadius * self.angularVelocity
if self.angularVelocity < sp.min_angular_velocity:
#print "*** clipping on MIN angular velocity ***"
self.angularVelocity = sp.min_angular_velocity
self.linearVelocity = self.approachRadius * self.angularVelocity
#self.angularVelocity = self.linearVelocity / sp.desired_radius + (sp.max_angular_velocity - sp.min_angular_velocity) * self.circularCoefToBeacon
elif(not self.hasBeacon and self.hasRobot): #Avoiding
self.status = 1
#coeficiente de proximidade
#calculado em funcao de (distancia minima, desejada e obtida), no intervalo [-0.5,0.5]
#multiplicado pela
self.proximityCoefToRobot = self.getProximityCoefToRobot()
self.propAngularCoefToRobot = self.getPropAngularCoefToRobot()
self.propDistCoefToRobot = self.getPropDistCoefToRobot()
self.linearVelocity = sp.linear_velocity + sp.linear_velocity * self.proximityCoefToRobot * self.propAngularCoefToRobot
self.angularVelocity = self.linearVelocity / self.obtainedDistanceToRobot + sp.angular_velocity * self.proximityCoefToRobot * self.propDistCoefToRobot
else: #status == "Seeking"
self.status = 0
self.linearVelocity = sp.linear_velocity
self.angularVelocity = sp.angular_velocity + sp.angular_velocity * np.random.random() * 0.1
### delimiter ###
self.linearVelocity = self.getDelimitedLinearVelocity(self.linearVelocity)
#self.angularVelocity = self.getDelimitedAngularVelocity(self.angularVelocity)
### Parada de Seguranca ###
self.linearVelocity = self.verifyMinDistances(numRobot, self.linearVelocity)
###------------------------------###
### Atualizacao das propriedades ###
myVelocities.angular = self.angularVelocity
myVelocities.linear = self.linearVelocity
#atualiza rotacoes e velocidades tangenciais #monitoramento
self.rightLinearVelocity = myVelocities.angular + myVelocities.linear
self.leftLinearVelocity = - 2 * myVelocities.linear - myVelocities.angular
self.rightWheelRotation = self.rightLinearVelocity / (sp.wheel_diameter * np.pi)
self.leftWheelRotation = self.leftLinearVelocity / (sp.wheel_diameter * np.pi)
#self.printAll(numRobot)
return myVelocities
def main(argv):
global robot, beacon, alien, twist
#ordem certa?
p1_pub = rospy.Publisher(sys.argv[1] + 'cmd_vel', Twist, queue_size=1)
rospy.init_node('p3dx_mover')
p1_twist = rospy.Subscriber(sys.argv[1] + "cmd_vel", Twist, getTwist)
p1_laser = rospy.Subscriber(sys.argv[1] + "p3dx/laser/scan", LaserScan,
callback)
print "Running for robot ", sys.argv[1], " - Please, wait ..."
prev, status = 0, 0
rate = rospy.Rate(3) #original 10hz
#instancias de representacoes locais simplificadas de coordenadas
rob = LinAng()
bea = LinAng()
ali = LinAng()
vel = LinAng()
debugTest = ""
while not rospy.is_shutdown():
#prev = statusChange(prev,status)
#status = getStatus()
#twist.linear.x = MIN_LINEAR_VEL
print ""
print "=" * 40
print ""
#se houver beacon no cone do sensor, pega valores diferentes de 0.0
if beacon.isBeacon():
bea.linear, bea.angular = beacon.center.getCoordPol()
debugTest = "| Beacon |"
#bea.prn()
else:
bea.linear = 0.0
bea.angular = 0.0
debugTest = "| |"
#se houver o robo no cone do sensor, pega valores diferentes de 0.0
if robot.isRobot():
rob.linear, rob.angular = robot.center.getCoordPol()
debugTest = debugTest + "| Robot |"
#rob.prn()
else:
rob.linear = 0.0
rob.angular = 0.0
debugTest = debugTest + "| |"
#se houver alien no cone do sensor, pega valores diferentes de 0.0
if alien.isAlien():
ali.linear, ali.angular = alien.center.getCoordPol()
debugTest = debugTest + "| Alien |"
#ali.prn()
else:
ali.linear = 0.0
ali.angular = 0.0
debugTest = debugTest + "| |"
#obtem a velocidade atual
vel.linear = twist.linear.x
vel.angular = twist.angular.z
#vel.prn()
print debugTest
#processa a circunavegacao
twist.linear.x, twist.angular.z = circ.process(vel, bea, rob,
ali).getVelocities()
#twist.linear.x = 0.25
#envie ao robo os valores das velocidades
p1_pub.publish(twist)
rate.sleep()
print "ending control.py..."
twist.angular.z = 0.0
twist.linear.x = 0.0
p1_pub.publish(twist)
exit()
def main(argv):
#global robot, beacon, alien, twist
quantRobots = int(sys.argv[1])
rospy.init_node('p3dx_mover')
#define a frequencia de atualizacao
rate = rospy.Rate(50) #original 10hz
#referente ao Beacon
p0_radius_to_target = rospy.Publisher("p0/radius_to_target", Float32, queue_size = 1)
sp_desired_radius_to_target = rospy.Publisher("sp/desired_radius_to_target", Float32, queue_size = 1)
p0_angle_to_target = rospy.Publisher("p0/angle_to_target", Float32, queue_size = 1)
sp_desired_angle_to_target = rospy.Publisher("sp/desired_angle_to_target", Float32, queue_size = 1)
p0_approach_radius_to_target = rospy.Publisher("p0/approach_radius_to_target", Float32, queue_size = 1)
p0_performed_radius = rospy.Publisher("p0/performed_radius", Float32, queue_size = 1)
#referente ao Robo (estaticos)
sp_minimal_distance = rospy.Publisher("sp/minimal_distance", Float32, queue_size = 1)
sp_desired_distance = rospy.Publisher("sp/desired_distance", Float32, queue_size = 1)
#dinamicos
p0_robot_distance = rospy.Publisher("p0/robot_distance", Float32, queue_size = 1)
p0_robot_angle = rospy.Publisher("p0/robot_angle", Float32, queue_size = 1)
#objetos detectados
p0_hasBeacon = rospy.Publisher("p0/hasBeacon", Int8, queue_size = 1)
p0_hasRobot = rospy.Publisher("p0/hasRobot", Int8, queue_size = 1)
p0_hasAlien = rospy.Publisher("p0/hasAlien", Int8, queue_size = 1)
#objects creation
for numRobot in range(0, quantRobots):
strNumRobot = 'p' + str(numRobot)
velocity_publishers.append(rospy.Publisher(strNumRobot + "/cmd_vel", Twist, queue_size = 1))
#twist_subscribers.append(rospy.Subscriber(strNumRobot + "/cmd_vel", Twist, getTwist))
odom_twist_subscribers.append(rospy.Subscriber(strNumRobot + "/odom", Odometry, getRealTwist, callback_args=numRobot))
laser_subscribers.append(rospy.Subscriber(strNumRobot + "/p3dx/laser/scan", LaserScan, getLaser, callback_args=numRobot))
print("Running for robot p%2d. Please, wait ..." % (numRobot))
robots.append(LocalObject(ID_TYPES))
beacons.append(LocalObject(ID_TYPES))
aliens.append(LocalObject(ID_TYPES))
ctrlCircum.append(circum())
ctrlTwists.append(Twist())
realTwists.append(Twist())
#instancias de representacoes locais simplificadas de coordenadas/velocidades
robots_coords.append(LinAng())
beacons_coords.append(LinAng())
aliens_coords.append(LinAng())
velocities.append(LinAng())
debug_msgs.append("")
#publicando os dados estaticos - removidos de dentro do while
sp_desired_radius_to_target.publish(sp.desired_radius)
#2017 sp_desired_angle_to_target.publish(sp.desired_angle_to_beacon)
sp_desired_angle_to_target.publish(sp.desired_angle_to_target)
sp_minimal_distance.publish(sp.min_distance_to_robot)
sp_desired_distance.publish(sp.desired_distance_to_robot)
while not rospy.is_shutdown():
#prev = statusChange(prev,status)
#status = getStatus()
#twist.linear.x = MIN_LINEAR_VEL
#print "---- While ---- "
#print "----------------"
for numRobot in range(0, quantRobots):
#envia comando a cada robo antes do controle rate.sleep()
#numRobotrint "numRobot", numRobot
#print "-" * 40
#print ""
#se houver beacon no cone do sensor, pega valores diferentes de 0.0
if beacons[numRobot].isBeacon():
#beacons_coords[numRobot].linear, beacons_coords[numRobot].angular = beacons[numRobot].center.getCoordPol()
beacons_coords[numRobot].setAllCoords(beacons[numRobot].center.getAllCoords())
debug_msgs[numRobot] = "| Beacon |"
#bea.prn()
else:
beacons_coords[numRobot].clear()
debug_msgs[numRobot] = "| |"
#se houver o robo no cone do sensor, pega valores diferentes de 0.0
if robots[numRobot].isRobot():
#robots_coords[numRobot].linear, robots_coords[numRobot].angular = robots[numRobot].center.getCoordPol()
robots_coords[numRobot].setAllCoords(robots[numRobot].center.getAllCoords())
debug_msgs[numRobot] = debug_msgs[numRobot] + "| Robot |"
#rob.prn()
else:
robots_coords[numRobot].clear()
debug_msgs[numRobot] = debug_msgs[numRobot] + "| |"
#se houver alien no cone do sensor, pega valores diferentes de 0.0
if aliens[numRobot].isAlien():
#aliens_coords[numRobot].setPolarCoords(aliens[numRobot].center.getCoordPol())
aliens_coords[numRobot].setAllCoords(aliens[numRobot].center.getAllCoords())
debug_msgs[numRobot] = debug_msgs[numRobot] + "| Alien |"
#ali.prn()
else:
aliens_coords[numRobot].clear()
debug_msgs[numRobot] = debug_msgs[numRobot] + "| |"
#obtem a velocidade atual
velocities[numRobot].setVelocities(realTwists[numRobot].linear.x, realTwists[numRobot].angular.z)
#vel.prn()
print debug_msgs[numRobot]
#obtem o horario atual
now = rospy.get_rostime()
#processa a circunavegacao
ctrlTwists[numRobot].linear.x, ctrlTwists[numRobot].angular.z = ctrlCircum[numRobot].getVelocities(numRobot, velocities[numRobot], beacons_coords[numRobot], robots_coords[numRobot], aliens_coords[numRobot], now).getVelocities()
#twist.linear.x = 0.25
#envie as informacoes de P0 para plotagem
if(numRobot == 0):
p0_radius_to_target.publish(ctrlCircum[numRobot].obtainedRadiusToBeacon)
p0_angle_to_target.publish(ctrlCircum[numRobot].obtainedAngleToBeacon)
p0_robot_angle.publish(ctrlCircum[numRobot].obtainedAngleToRobot)
p0_robot_distance.publish(ctrlCircum[numRobot].obtainedDistanceToRobot)
p0_approach_radius_to_target.publish(ctrlCircum[numRobot].approachRadius)
p0_hasRobot.publish(ctrlCircum[numRobot].hasRobot)
p0_hasBeacon.publish(ctrlCircum[numRobot].hasBeacon)
p0_hasAlien.publish(ctrlCircum[numRobot].hasAlien)
pRadius = NaN
if velocities[numRobot].angular != 0.0:
pRadius = velocities[numRobot].linear/ velocities[numRobot].angular
#print "Real Velocity : vL =", velocities[numRobot].linear, " vA =", velocities[numRobot].angular, "pRadius =", pRadius
p0_performed_radius.publish(pRadius)
#envie ao robo os valores das velocidades
velocity_publishers[numRobot].publish(ctrlTwists[numRobot])
#for numRobot in range(0, quantRobots):
#print "P", numRobot, "vl: ", twists[numRobot].linear.x, ", va: ", twists[numRobot].angular.z
#estabelece uma pausa para manter a frequencia definida
rate.sleep()
print "ending control.py..."
#twist.angular.z = 0.0
#twist.linear.x = 0.0
#p1_pub.publish(twist)
exit()
def __init__(self):
#Raio obtido
self.obtainedRadiusToTarget = 0.0
#Angulo Obtido em Relacao ao Target
self.obtainedAngleToTarget = 0.0
#Angulo Obtido em Relacao ao Robo
self.obtainedAngleToRobot = 0.0
#Distancia obtida
self.obtainedDistanceToRobot = 0.0
#Distancia obtida ao alien
self.obtainedDistanceToAlien = 0.0
#angulo obtido ao alien
self.obtainedAngleToAlien = 0.0
#nova dimensao
self.obtainedDistanceFromTargetToRobot = 0.0
#novo controle
self.circularCoefToTarget = 0.0
#novo controle
self.propDistCoefToRobot = 0.0
#novo controle
self.linearCoefTargetToRobot = 0.0
#novo controle
self.propAngularCoefToRobot = 0.0
#novo controle
self.linearCoefToTarget = 0.0
#self
self.proximityCoefToRobot = 0.0
#self
self.actualTime = 0.0
#self
self.previousTime = 0.0
#self
self.approachRadius = 0.0
#self
self.projectedDistancePoint = LinAng()
#localizacao x,y , com base no frame centralizado em mim
self.target = LinAng()
self.robot = LinAng()
self.alien = LinAng()
#substituida por linear velocity, angular_velocity
self.linearVelocity = sp.linear_velocity
self.angularVelocity = sp.angular_velocity
self.realLinearVelocity = 0.0
self.realAngularVelocity = 0.0
#velocidade tangencial/linear da roda direita e esquerda #m/s
self.rightLinearVelocity = 0.0
self.leftLinearVelocity = 0.0
#rotacao da roda direita e esquerda #r.p.s
self.rightWheelRotation = 0.0
self.leftWheelRotation = 0.0
#Target detectado?
self.hasTarget = False
#Robo detectado?
self.hasRobot = False
#Alien detectado?
self.hasAlien = False
#Status
self.status = 0
#pids
self.pidLinear = PID(3.0, 4.0, 1.2)
self.pidLinear.setPoint(sp.desired_radius)
self.pidAngular = PID(3.0, 4.0, 1.2)
self.pidAngular.setPoint(sp.desired_angle_to_target)
#mutable circumnavigation appRadiusCoef
self.desired_radius = sp.desired_radius
class circum:
def __init__(self):
#Raio obtido
self.obtainedRadiusToTarget = 0.0
#Angulo Obtido em Relacao ao Target
self.obtainedAngleToTarget = 0.0
#Angulo Obtido em Relacao ao Robo
self.obtainedAngleToRobot = 0.0
#Distancia obtida
self.obtainedDistanceToRobot = 0.0
#Distancia obtida ao alien
self.obtainedDistanceToAlien = 0.0
#angulo obtido ao alien
self.obtainedAngleToAlien = 0.0
#nova dimensao
self.obtainedDistanceFromTargetToRobot = 0.0
#novo controle
self.circularCoefToTarget = 0.0
#novo controle
self.propDistCoefToRobot = 0.0
#novo controle
self.linearCoefTargetToRobot = 0.0
#novo controle
self.propAngularCoefToRobot = 0.0
#novo controle
self.linearCoefToTarget = 0.0
#self
self.proximityCoefToRobot = 0.0
#self
self.actualTime = 0.0
#self
self.previousTime = 0.0
#self
self.approachRadius = 0.0
#self
self.projectedDistancePoint = LinAng()
#localizacao x,y , com base no frame centralizado em mim
self.target = LinAng()
self.robot = LinAng()
self.alien = LinAng()
#substituida por linear velocity, angular_velocity
self.linearVelocity = sp.linear_velocity
self.angularVelocity = sp.angular_velocity
self.realLinearVelocity = 0.0
self.realAngularVelocity = 0.0
#velocidade tangencial/linear da roda direita e esquerda #m/s
self.rightLinearVelocity = 0.0
self.leftLinearVelocity = 0.0
#rotacao da roda direita e esquerda #r.p.s
self.rightWheelRotation = 0.0
self.leftWheelRotation = 0.0
#Target detectado?
self.hasTarget = False
#Robo detectado?
self.hasRobot = False
#Alien detectado?
self.hasAlien = False
#Status
self.status = 0
#pids
self.pidLinear = PID(3.0, 4.0, 1.2)
self.pidLinear.setPoint(sp.desired_radius)
self.pidAngular = PID(3.0, 4.0, 1.2)
self.pidAngular.setPoint(sp.desired_angle_to_target)
#mutable circumnavigation appRadiusCoef
self.desired_radius = sp.desired_radius
def setTime(self, time):
self.previousTime = self.actualTime
self.actualTime = time
def getPropAngularCoefToTarget(self):
#sensor_angle, obtained_angle, desired_angle
#calculo o coeficiente proporcional angular em relacao do angulo ao Target
#envolve angulo do sensor, angulo desejado e angulo obtido
#retorna 0.0 quando angulo_obtido = angulo_desejado
#intervalo de retorno [-1.0, 1.0] *** plotar arquivo: "proportionalAngularToTarget.py"
#valor fixo
if self.target.angular <= sp.min_ctrl_angle:
#print "self.target.angular <= sp.min_ctrl_angle"
return -1.0
#apenas semantica
elif self.target.angular >= sp.max_ctrl_angle:
#print "self.target.angular >= sp.max_ctrl_angle"
return 1.0
#entao, min_ctrl_angle < self.target.angular < max_ctrl_angle
else:
#print "min_ctrl_angle < self.target.angular < max_ctrl_angle"
return 2 * (self.target.angular - sp.min_ctrl_angle) / (
sp.max_ctrl_angle - sp.min_ctrl_angle) - 1
#2 * (a - min)/(max - min)
def getPropRadialCoefToTarget(self):
#sensor_radius, obtained_radius, desired_radius
#calcula o valor do coeficiente com base no raio ao Target
#envolve raio do sensor, raio desejado e raio obtido
#retorna 0.0 quando raio_obtido = raio_desejado
#intervalo de retorno [-1.0, 1.0] *** plotar arquivo: "proportionalRadialToTarget.py"
if self.target.linear >= sp.desired_radius:
#y.flat[high] = (2 * (x - dR)/(sR - dR) + 1) / 2
#print "self.target.linear >= sp.desired_radius"
return (2 * (self.target.linear - sp.desired_radius) /
(sp.sensor_cone_radius - sp.desired_radius) + 1) / 2
else:
#print "self.target.linear < sp.desired_radius"
return (self.target.linear - sp.desired_radius) / (
sp.desired_radius - sp.min_distance_to_robot)
#y.flat[low] = (x - dR)/(dR - mD)
def getCircularCoefToTarget(self):
#calcula o coeficiente para circular o target
#quando ha o Target, retorna a media dos valores obtidos por getProportionalAngularToTarget e getProportionalRadialToTarget
if self.target.angular == 0.0:
return 0.0
return (self.getPropAngularCoefToTarget() +
self.getPropRadialCoefToTarget()) / 2.0
#def getPropAngularCoefToRobot(self):
#desativado pelo desuso 2017 08 08
#calcula o valor do coeficiente em relacao ao angulo ao robo (evitar colisao)
#envolve o angulo obtido e o angulo do sensor
#retorna valor [0.0, 1.0] *** plotar arquivo "getPropAngularCoefToRobot.py"
#abs_angle_dif = abs(self.obtainedAngleToRobot) #apenas valores positivos
#return (1 - angle_to_robot / (sp.sensor_cone_angle / 2.0))**2
def getPropAngularCoefToRobot(self):
#utilizado para desviar do robo circunavegando
#intervalo de retorno [-1.0, 1.0]
#return 2 * (self.robot.angular - sp.min_ctrl_angle) / (sp.max_ctrl_angle - sp.min_ctrl_angle) - 1
#wolfram alpha: f(x)=(1 - |x|/(3 * pi/4))^4, x = -3pi/4 to 3pi/4
pactr = (1.0 - (abs(self.robot.angular) /
(sp.sensor_cone_angle / 2.0)))**6.0
return pactr
def getInterfAngularProjected(self):
#calcula o angulo projetado ao robo
#utiliza o R descrito pelo robo
#retorna 1.0, para angulo projetado = angulo detectado (interferencia maxima)
#retorna valor proporcional < 1.0
iap = 1.0
if (self.angularVelocity == 0.0):
return 1.0
described_radius = self.linearVelocity / self.angularVelocity
if (abs(described_radius) > sp.desired_distance_to_robot / 2.0):
cos = (sp.desired_distance_to_robot / 2.0) / described_radius
#print "cos: ", cos
firstAngle = math.acos(cos)
#print "firstAngle(rad): ", firstAngle
firstAngle = firstAngle * 180 / math.pi
#print "firstAngle(grades): ", firstAngle
projectedAngle = 90.0 - firstAngle
print "projectedAngle:", projectedAngle
print "obtainedAngle:", self.obtainedAngleToRobot
iap = abs(self.obtainedAngleToRobot -
projectedAngle) / sp.sensor_cone_angle
return iap
# def getPropDistCoefToRobot(self):
# #calcula coeficiente linear relacao a posicao do robo
# #apenas se a distancia obtida for menor do que a desejada
# #envolve a distancia obtida e a desejada em relacao ao robot
# #retorna valores no intervalo [0.0,1.0] *** plotar arquivo "getPropDistCoefToRobot"
# if(self.obtainedDistanceToRobot < sp.desired_distance_to_robot):
# return 1 - self.obtainedDistanceToRobot / sp.desired_distance_to_robot
# else:
# return 0.0
def getPropDistCoefToRobot(self):
#calcula coeficiente linear relacao a posicao do robo
#apenas se a distancia obtida for menor do que a desejada
#envolve a distancia obtida e a desejada em relacao ao robot
#retorna valores no intervalo [0.0,1.0] *** plotar arquivo "getPropDistCoefToRobot2"
if (self.obtainedDistanceToRobot < sp.desired_distance_to_robot):
return 1 - self.obtainedDistanceToRobot / sp.desired_distance_to_robot
else:
return 0.0
# def getLinearCoefTargetToRobot(self, target, robot):
# #calcula o coeficiente linear em relacao a distancia do target ao robo detectado
# #plotar arquivo getLinearCoefTargetToRobot.py
# #distancia do target ao robot
# obtainedDistanceFromTargetToRobot = ((target.x - robot.x)**2 + (target.y - robot.y)**2)**0.5
# #atualiza atributo
# self.obtainedDistanceFromTargetToRobot = self.H(self.hasRobot and self.hasTarget, True) * obtainedDistanceFromTargetToRobot
#
# minObtainedDistance = sp.desired_radius - sp.min_distance_to_robot
# maxObtainedDistance = sp.desired_radius + sp.min_distance_to_robot
#
# if ((minObtainedDistance < obtainedDistanceFromTargetToRobot) and (obtainedDistanceFromTargetToRobot < maxObtainedDistance )):
# modularDistance = ((obtainedDistanceFromTargetToRobot - sp.desired_radius)**2)**0.5
# return 1 - modularDistance / sp.min_distance_to_robot
# else:
# return 0.0
def getProximityCoefToRobot(self):
#calcula o coeficiente de proximidade em relacao ao robo mais proximo
#envolve a distancia obtida, distancia desejada e minima distancia entre robos
#retorna [-0.5,0.5] *** plotar arquivo "getProximityCoefToRobot.py"
#quando a distancia desejada for igual obtida, retorna 0
if self.obtainedDistanceToRobot > 2 * sp.desired_distance_to_robot:
return 0.5
if self.obtainedDistanceToRobot < sp.min_distance_to_robot:
return -0.5
#senao, calcule y = (x - dD) / (2 * (dD - mD))
return (self.obtainedDistanceToRobot - sp.desired_distance_to_robot
) / (2 *
(sp.desired_distance_to_robot - sp.min_distance_to_robot))
def H(self, value, reference):
#Heaviside function
if value >= reference:
return 1.0
else:
return 0.0
def printAll(self, num_id):
#print "[", num_id, "]:: Ci:", self.interferenceCoef, "Cc:", self.conversionCoef, "Co", self.orientationCoef, "Cp", self.proximityCoefToRobot, "Ca", self.forwardCoef
print "-------- P", num_id, " ---------"
#print("Robot Real Velocities (vL,vA): %6.2f m/s, %6.2f rad/s " % (self.realLinearVelocity,self.realAngularVelocity))
#print("p3dx_mover velocities (vL,vA): %6.2f m/s, %6.2f rad/s " % (self.linearVelocity,self.angularVelocity))
#print("Velocidade Tang Direita : %6.2f m/s" % (self.rightLinearVelocity))
#print("Rotacao Roda Direita : %6.2f rad/s" % (self.rightWheelRotation))
#print("Velocidade Tang Esquerda : %6.2f m/s" % (self.leftLinearVelocity))
#print("Rotacao Roda Esquerda : %6.2f rad/s" % (self.leftWheelRotation))
#print("Target x,y : %6.2f, %6.2f " % (self.target))
#print( "Target (Raio, Angulo): %6.2f, %6.2f " % (self.obtainedRadiusToTarget, self.obtainedAngleToTarget))
#print("Raio do Sensor (Sr): %6.2f" % (sp.sensor_cone_radius))
#print("Raio desejado (dR): %6.2f" % (sp.desired_radius))
#print("Circular Coef Target aCtB: %6.2f" % (self.circularCoefToTarget))
#print("Robot x,y : %6.2f, %6.2f " % (self.robot))
#print(" Robot (Dist, Angulo) : %6.2f, %6.2f" % (self.obtainedDistanceToRobot, self.obtainedAngleToRobot))
#print("Min Distancia entre Rob(mD): %6.2f" % (sp.min_distance_to_robot))
#print("Distancia desejada (dD): %6.2f" % (sp.desired_distance_to_robot))
#print("proximityCoefToRobot : %6.2f " % (self.proximityCoefToRobot))
#print("propAngularCoefToRobot : %6.2f" % (self.propAngularCoefToRobot))
#print("propDistCoefToRobot : %6.2f" % (self.propDistCoefToRobot))
#print("Distancia Target to Robot : %6.2f" % (self.obtainedDistanceFromTargetToRobot))
#print("Ang. Coef Target to Robot : %6.2f" % (self.linearCoefTargetToRobot))
#print sp.status_msg[self.status], self.status
#print ""
print ""
#atualiza a existencia de objetos detectados
def updateDetectedObjects(self, detectedTargetDist, detectedRobotDist,
detectedAlienDist):
if detectedTargetDist.linear > 0.0:
self.hasTarget = True
self.obtainedRadiusToTarget, self.obtainedAngleToTarget = detectedTargetDist.getPolarCoords(
)
self.target.setRetCoords(detectedTargetDist.getRetCoords())
self.target.setPolarCoords(detectedTargetDist.getPolarCoords())
else:
self.hasTarget = False
self.obtainedRadiusToTarget, self.obtainedAngleToTarget = 0.0, 0.0
self.target.clear()
if detectedRobotDist.linear > 0.0:
self.hasRobot = True
self.obtainedDistanceToRobot, self.obtainedAngleToRobot = detectedRobotDist.getPolarCoords(
)
self.robot.setRetCoords(detectedRobotDist.getRetCoords())
self.robot.setPolarCoords(detectedRobotDist.getPolarCoords())
else:
self.hasRobot = False
self.obtainedDistanceToRobot, self.obtainedAngleToRobot = 0.0, 0.0
self.robot.clear()
if detectedAlienDist.linear > 0.0:
self.hasAlien = True
self.obtainedDistanceToAlien, self.obtainedAngleToAlien = detectedAlienDist.getPolarCoords(
)
self.alien.setRetCoords(detectedAlienDist.getRetCoords())
self.alien.setPolarCoords(detectedAlienDist.getPolarCoords())
else:
self.hasAlien = False
self.obtainedDistanceToAlien, self.obtainedAngleToAlien = 0.0, 0.0
self.alien.clear()
#devolve as velocidades linear e angular
def getVelocities(self, numRobot, myVelocities, targetCoord, robotCoord,
alienCoord, now):
#print "target coord", targetCoord.getVelocities()
#print "robot coord", robotCoord.getVelocities()
#print "alien coord", alienCoord.getVelocities()
self.setTime(now.secs + now.nsecs / (10**9)) # segundos.milisegundos
#print ("Tempo decorrido %4.9f" %(self.actualTime - self.previousTime))
#atualiza a velocidade real
self.realLinearVelocity, self.realAngularVelocity = myVelocities.getVelocities(
)
#atualiza a existencia dos objetos
self.updateDetectedObjects(targetCoord, robotCoord, alienCoord)
#age conforme os objetos detectados
if (self.hasTarget): #Circulating and not self.hasRobot
#self.status = 2
pact = self.getPropAngularCoefToTarget()
pactr = self.getPropAngularCoefToRobot()
print "pactr: ", pactr
#mlv = sp.max_linear_velocity
#rconv = 0.0
propAng = (sp.desired_angle_to_target - self.obtainedAngleToTarget
) / (sp.desired_angle_to_target -
(-sp.sensor_cone_angle / 2))
#iap = self.getInterfAngularProjected()**5
if (pact == -1.0):
self.desired_radius = self.obtainedRadiusToTarget - sp.desired_radius + sp.wheel_separation / 2
if (self.hasRobot):
self.desired_radius = (
(self.desired_radius + self.robot.linear) /
2.0) * (1 - pactr)
self.linearVelocity = sp.max_linear_velocity * propAng
self.angularVelocity = -(self.linearVelocity /
self.desired_radius)
else:
self.desired_radius = sp.desired_radius + sp.wheel_separation / 2.0
if (self.hasRobot):
self.desired_radius = (
(self.desired_radius + self.robot.linear) /
2.0) * (1 - pactr)
self.linearVelocity = sp.max_linear_velocity
self.angularVelocity = (self.linearVelocity /
self.desired_radius) * (1 + pact)
#if(self.hasRobot):
#pactr = self.getPropAngularCoefToRobot()
#self.desired_radius = ((self.desired_radius + self.robot.linear)/2.0) * (1 - pactr)
#self.angularVelocity = (self.linearVelocity / self.desired_radius) * (1 + pactr)
#print "iap:", iap
print "pact:", pact
#print "rconv:", rconv
print "propAng", propAng
print "linearVelocity", self.linearVelocity
print "angularVelocity", self.angularVelocity
#print "pidLinear : ", self.pidLinear.update(self.obtainedRadiusToTarget)
#print "pidAngular: ", self.pidAngular.update(self.obtainedAngleToTarget)
#print "vel angular: ", self.angularVelocity
#elif(self.hasTarget and self.hasRobot): #Escorting
#self.status = 3
#self.linearVelocity = 1.0
elif (not self.hasTarget and self.hasRobot): #Avoiding
#abordagem que diminui a velocidade linear para desviar
#iap = self.getInterfAngularProjected()
#self.linearVelocity = self.linearVelocity * iap
print "robot.angular", self.robot.angular
#abordagem que circunavega o outro robo em rotacao inversa
pactr = self.getPropAngularCoefToRobot()
print "pactr: ", pactr
self.desired_radius = (
(self.desired_radius + self.robot.linear) / 2.0) * (1 - pactr)
#if(self.robot.angular > 0.0):
#self.desired_radius += -1
print "desired_radius: ", self.desired_radius
#self.linearVelocity = sp.max_linear_velocity
self.angularVelocity = (self.linearVelocity /
self.desired_radius) * (1 + pactr)
print "angularVelocity: ", self.angularVelocity
else: #status == "Seeking"
self.status = 0
self.linearVelocity = sp.linear_velocity
decimal_random_rate = np.random.random() / 10.0
self.angularVelocity = sp.angular_velocity * (1 +
decimal_random_rate)
###------------------------------###
### Atualizacao das propriedades ###
myVelocities.angular = self.angularVelocity
myVelocities.linear = self.linearVelocity
#atualiza rotacoes e velocidades tangenciais #monitoramento
self.rightLinearVelocity = myVelocities.angular + myVelocities.linear
self.leftLinearVelocity = -2 * myVelocities.linear - myVelocities.angular
self.rightWheelRotation = self.rightLinearVelocity / (
sp.wheel_diameter * np.pi)
self.leftWheelRotation = self.leftLinearVelocity / (sp.wheel_diameter *
np.pi)
#self.printAll(numRobot)
return myVelocities
class circum:
def __init__(self):
#Raio obtido
self.obtainedRadiusToTarget = 0.0
#Angulo Obtido em Relacao ao Target
self.obtainedAngleToTarget = 0.0
#Angulo Obtido em Relacao ao Robo
self.obtainedAngleToRobot = 0.0
#Distancia obtida
self.obtainedDistanceToRobot = 0.0
#Distancia obtida ao alien
self.obtainedDistanceToAlien = 0.0
#angulo obtido ao alien
self.obtainedAngleToAlien = 0.0
#nova dimensao
self.obtainedDistanceFromTargetToRobot = 0.0
#novo controle
self.circularCoefToTarget = 0.0
#novo controle
self.propDistCoefToRobot = 0.0
#novo controle
self.linearCoefTargetToRobot = 0.0
#novo controle
self.propAngularCoefToRobot = 0.0
#novo controle
self.linearCoefToTarget = 0.0
#self
self.proximityCoefToRobot = 0.0
#self
self.actualTime = 0.0
#self
self.previousTime = 0.0
#self
self.approachRadius = 0.0
#self
#self.vRobot = LinAng()
#localizacao x,y , com base no frame centralizado em mim
self.target = LinAng()
self.robot = LinAng()
self.alien = LinAng()
#substituida por linear velocity, angular_velocity
self.linearVelocity = sp.linear_velocity
self.angularVelocity = sp.angular_velocity
self.realLinearVelocity = 0.0
self.realAngularVelocity = 0.0
#velocidade tangencial/linear da roda direita e esquerda #m/s
self.rightLinearVelocity = 0.0
self.leftLinearVelocity = 0.0
#rotacao da roda direita e esquerda #r.p.s
self.rightWheelRotation = 0.0
self.leftWheelRotation = 0.0
#Target detectado?
self.hasTarget = False
#Robo detectado?
self.hasRobot = False
#Alien detectado?
self.hasAlien = False
#Status
self.status = 0
#pids
self.pidLinear = PID(3.0, 4.0, 1.2)
self.pidLinear.setPoint(sp.desired_radius)
self.pidAngular = PID(3.0, 4.0, 1.2)
self.pidAngular.setPoint(sp.desired_angle_to_target)
#mutable circumnavigation appRadiusCoef
self.desired_radius = sp.desired_radius
#added in 12dez2018
self.light_factor = 0.0
def setTime(self, time):
self.previousTime = self.actualTime
self.actualTime = time
def getPropInterFromRobot(self, target_to_robot_distance): #pifR #\iota
#gera um trapezio
#utilizada para medir a crenca na circunavegacao do robo
#com base na distancia x entre o alvo e o robo
#retorna 1.0 quando x estiver entre minima distancia e raio desejado
#retorna proporcional quando x < minima distancia
#retorna proporcional quando x > raio desejado
#retorna 0,0 quando a distancia for 0 ou maior que raio do sensor
x = target_to_robot_distance
a = 0.0
b = sp.min_distance_to_robot
c = sp.desired_radius
d = sp.sensor_cone_radius
return max(min((x-a)/(b-a), 1.0, (d-x)/(d-c)), 0.0)
def getPropDistToRobot(self): #pdtR
#gera uma valor de erro proporcional a distancia desejada ao robo
#utilizado para incrementar as velocidades linear e angular
#se obtida = desejada, pde = 1.0
#se obtida > desejada, pde = 1.xxxx
#se obtida < desejada, pde = 0.9xxx
if(self.obtainedDistanceToRobot == sp.sensor_cone_radius):
return 1.0
return (sp.sensor_cone_radius + self.obtainedDistanceToRobot)/(sp.sensor_cone_radius + sp.desired_distance_to_robot)
def getDistanceFromTargetToRobot(self): #dfTtR
#retorna a distancia do alvo ao robo, quando houver os dois
if((not self.hasRobot) or (not self.hasTarget)):
return 0.0
else:
return ((self.target.x - self.robot.x)**2 + (self.target.y - self.robot.y)**2)**0.5
def getPropAngularCoefToTarget(self): #pactT #_delta
#sensor_angle, obtained_angle, desired_angle
#calcula o coeficiente proporcional angular em relacao do angulo ao Target
#envolve angulo do sensor, angulo desejado e angulo obtido
#retorna 0.0 quando angulo_obtido = angulo_desejado
#intervalo de retorno [-1.0, 1.0] *** plotar arquivo: "proportionalAngularToTarget.py"
#valor fixo
if self.target.angular <= sp.min_ctrl_angle:
#print "self.target.angular <= sp.min_ctrl_angle"
return -1.0
#apenas semantica
elif self.target.angular >= sp.max_ctrl_angle:
#print "self.target.angular >= sp.max_ctrl_angle"
return 1.0
#entao, min_ctrl_angle < self.target.angular < max_ctrl_angle
else:
#print "min_ctrl_angle < self.target.angular < max_ctrl_angle"
return 2 * (self.target.angular - sp.min_ctrl_angle) / (sp.max_ctrl_angle - sp.min_ctrl_angle) - 1
#2 * (a - min)/(max - min)
def getPropRadialCoefToTarget(self): #prctT
#nova
#retorna 1.0 quando raio obtido = desejado
#retorna uma curva de tangente hiperbolica
#assintota negativa para 0
#assintota positiva para 2
#https://www.wolframalpha.com/input/?i=plot(1+%2B+(1+-+e%5E(-(x-3%2F2)*k))+%2F+(1+%2B+e%5E(-(x-3%2F2)*k))),+0+%3C+x+%3C+3,++k%3D6
d = self.obtainedRadiusToTarget - sp.desired_radius
cp.cyan("d: " + str(d))
k = sp.sensor_cone_radius / sp.min_distance_to_robot
cp.cyan("k: " + str(k))
m = math.e**(-d*k)
cp.cyan("m: " + str(m))
prctT = 1 - (1-m)/(1+m)
cp.cyan("prctT: " + str(prctT))
return prctT
def getPropAngularCoefToRobot(self): #pactR
#utilizado para desviar do robo circunavegando
#intervalo de retorno [-1.0, 1.0]
#return 2 * (self.robot.angular - sp.min_ctrl_angle) / (sp.max_ctrl_angle - sp.min_ctrl_angle) - 1
#wolfram alpha: f(x)=(1 - |x|/(3 * pi/4))^4, x = -3pi/4 to 3pi/4
#<https://www.wolframalpha.com/input/?i=f(x)%3D(1+-+%7Cx%7C%2F(3+*+pi%2F4))%5E4,+x+%3D+-3pi%2F4+to+3pi%2F4>
pactR = (1.0 - (abs(self.robot.angular) / (sp.sensor_cone_angle / 2.0)))**6.0
return pactR
def getInterfAngularProjToRobot(self): #iaptR
#calcula o angulo projetado ao robo
#utiliza o R descrito pelo robo
#retorna 1.0, para angulo projetado = angulo detectado (interferencia maxima)
#retorna valor proporcional < 1.0
iaptR = 1.0
if(self.angularVelocity == 0.0):
return 1.0
r = self.linearVelocity / self.angularVelocity
if(abs(r) > sp.desired_distance_to_robot/2.0):
fcos = (sp.desired_distance_to_robot / 2.0) / r
cp.cyan("cos: " + str(fcos))
f = math.acos(fcos)
cp.cyan("firstAngle(rad): " + str(fcos))
g = f * 180 / math.pi
cp.cyan("firstAngle(grades): " + str(g))
p = 90.0 - g
cp.cyan("projectedAngle (90 - g):" + str(p))
iaptR = abs(self.obtainedAngleToRobot - p)/sp.sensor_cone_angle
cp.cyan("iaptR" + str(iaptR))
return iaptR
def getPropDistCoefToRobot(self): #pdctR
#calcula coeficiente linear relacao a posicao do robo
#apenas se a distancia obtida for menor do que a desejada
#envolve a distancia obtida e a desejada em relacao ao robot
#retorna valores no intervalo [0.0,1.0] *** plotar arquivo "rho.py"
if(self.obtainedDistanceToRobot < sp.desired_distance_to_robot):
return 1 - self.obtainedDistanceToRobot / sp.desired_distance_to_robot
else:
return 0.0
def getProportionalAngleToTarget(self): #patT
num = sp.desired_angle_to_target - self.obtainedAngleToTarget
den = sp.sensor_cone_angle/2
#wolfram alpha f(x)=(pi/2 - x)/( 3pi/4), x=-3pi/4 to 3pi/4
return num/den
def getProximityCoefToRobot(self): #pctR
#calcula o coeficiente de proximidade em relacao ao robo mais proximo
#envolve a distancia obtida, distancia desejada e minima distancia entre robos
#retorna [-0.5,0.5] *** plotar arquivo "getProximityCoefToRobot.py"
#quando a distancia desejada for igual obtida, retorna 0
if self.obtainedDistanceToRobot > 2 * sp.desired_distance_to_robot:
return 0.5
if self.obtainedDistanceToRobot < sp.min_distance_to_robot:
return -0.5
#senao, calcule y = (x - dD) / (2 * (dD - mD))
return (self.obtainedDistanceToRobot - sp.desired_distance_to_robot) / (2 * (sp.desired_distance_to_robot - sp.min_distance_to_robot))
def H(self, value, reference):
#Heaviside function
if value >= reference:
return 1.0
else:
return 0.0
def printAll(self, num_id):
#print "[", num_id, "]:: Ci:", self.interferenceCoef, "Cc:", self.conversionCoef, "Co", self.orientationCoef, "Cp", self.proximityCoefToRobot, "Ca", self.forwardCoef
print "-------- P", num_id, " ---------"
print ""
#atualiza a existencia de objetos detectados
def updateDetectedObjects(self, detectedTargetDist, detectedRobotDist, detectedAlienDist):
if detectedTargetDist.linear > 0.0:
self.hasTarget = True
self.obtainedRadiusToTarget, self.obtainedAngleToTarget = detectedTargetDist.getPolarCoords()
self.target.setRetCoords(detectedTargetDist.getRetCoords())
self.target.setPolarCoords(detectedTargetDist.getPolarCoords())
else:
self.hasTarget = False
#self.obtainedRadiusToTarget, self.obtainedAngleToTarget = 0.0, 0.0
self.obtainedRadiusToTarget, self.obtainedAngleToTarget = sp.sensor_cone_radius, sp.sensor_cone_angle
self.target.clear()
if detectedRobotDist.linear > 0.0:
self.hasRobot = True
self.obtainedDistanceToRobot, self.obtainedAngleToRobot = detectedRobotDist.getPolarCoords()
self.robot.setRetCoords(detectedRobotDist.getRetCoords())
self.robot.setPolarCoords(detectedRobotDist.getPolarCoords())
else:
self.hasRobot = False
#self.obtainedDistanceToRobot, self.obtainedAngleToRobot = 0.0, 0.0
self.obtainedDistanceToRobot, self.obtainedAngleToRobot = sp.sensor_cone_radius, sp.sensor_cone_angle
self.robot.clear()
if detectedAlienDist.linear > 0.0:
self.hasAlien = True
self.obtainedDistanceToAlien, self.obtainedAngleToAlien = detectedAlienDist.getPolarCoords()
self.alien.setRetCoords(detectedAlienDist.getRetCoords())
self.alien.setPolarCoords(detectedAlienDist.getPolarCoords())
else:
self.hasAlien = False
#self.obtainedDistanceToAlien, self.obtainedAngleToAlien = 0.0, 0.0
self.obtainedDistanceToAlien, self.obtainedAngleToAlien = sp.sensor_cone_radius, sp.sensor_cone_angle
self.alien.clear()
#devolve as velocidades linear e angular
def getVelocities(self, numRobot , myVelocities, targetCoord, robotCoord, alienCoord, lightFactor, now):
#print "target coord", targetCoord.getVelocities()
#print "robot coord", robotCoord.getVelocities()
#print "alien coord", alienCoord.getVelocities()
self.light_factor = lightFactor
self.setTime(now.secs + now.nsecs / (10**9)) # segundos.milisegundos
#print ("Tempo decorrido %4.9f" %(self.actualTime - self.previousTime))
#atualiza a velocidade real
self.realLinearVelocity, self.realAngularVelocity = myVelocities.getVelocities()
#atualiza a existencia dos objetos
self.updateDetectedObjects(targetCoord, robotCoord, alienCoord)
#age conforme os objetos detectados
pactT = self.getPropAngularCoefToTarget() #[-1.0...1.0]
#cp.target("pactT : " + str(pactT))
patT = self.getProportionalAngleToTarget() #[negativo, para angulo > 90.. positivo para angulo < 90
#cp.target("patT : " + str(patT))
pacTR = self.getPropAngularCoefToRobot() #[-1.0...1.0]
#cp.robot("pacTR : " + str(pacTR))
dfTtR = self.getDistanceFromTargetToRobot()
#cp.robot("dfTtR : " + str(dfTtR))
pdtR = self.getPropDistToRobot() #[---, 1.0, +++]
#cp.robot("pdtR : " + str(pdtR))
pifR = self.getPropInterFromRobot(dfTtR) #trapezio [1.0 quando Dmin <= x <= Dd], [0.0 quando x > rS]
#cp.robot("pifR : " + str(pifR))
pdctR = self.getPropDistCoefToRobot()
#cp.robot("pdctR : " + str(pdctR))
#heaviside following
hSideFollow = (0.0 if (self.light_factor == 0.0) else 1.0)
vLinFollow = sp.max_linear_velocity
vAngFollow = self.light_factor/2.0
#heaviside wandering eh 0.0 se um dos dois forem detectados ou se houver alteracao em light_sensor
hSideWander = (0.0 if (self.light_factor != 0.0 or self.obtainedRadiusToTarget < sp.sensor_cone_radius or self.obtainedDistanceToRobot < sp.sensor_cone_radius) else 1.0)
#cp.wander("hSideWander: " + str(hSideWander) + " (heaviside Wander)")
vLinWander = sp.linear_velocity
#cp.wander("vLinWander: " + str(vLinWander) + " (linear velocity to Wander)")
radiusWander = sp.linear_velocity / (sp.angular_velocity * (1 + (np.random.random() / 10.0)))
#cp.wander("radiusWander: " + str(radiusWander) + " (radius to Wander)")
hSideTarget = (1.0 if self.obtainedRadiusToTarget < sp.sensor_cone_radius else 0.0)
#cp.target("hSideTarget: " + str(hSideTarget) + " (heaviside Target)")
hSideRobot = (1.0 if self.obtainedDistanceToRobot < sp.sensor_cone_radius else 0.0)
#cp.robot("hSideRobot: " + str(hSideRobot) + " (heaviside Robot)")
#vLinTarget = sp.min_linear_velocity + sp.delta_linear_velocity * patT
vLinTarget = sp.max_linear_velocity
if(pactT == -1):
vLinTarget = vLinTarget * patT
#cp.target("vLinTarget: " + str(vLinTarget) + " (linear velocity to Target)")
#if pacT == -1 ... self.linearVelocity = sp.min_linear_velocity + sp.delta_linear_velocity * patT
# else self.linearVelocity = sp.min_linear_velocity + sp.delta_linear_velocity * bic * pde
#vLinRobot = sp.min_linear_velocity + sp.delta_linear_velocity * pifR**2 * pdtR
vLinRobot = sp.max_linear_velocity * (pdctR * pdtR)**pifR
#cp.robot("vLinRobot: " + str(vLinRobot) + " (linear velocity to Robot)")
#irtT = (0.0 if pactT == -1 else self.obtainedRadiusToTarget)
#cp.target("irtt: " + str(irtT) + " (inverse radius to Target)")
#drtT = (sp.desired_radius if pactT == -1 else sp.desired_radius * (-1))
#cp.target("drtT: " + str(drtT) + " (desired radius to Target)")
radiusTarget = sp.wheel_separation / 2.0
if(pactT == -1):
radiusTarget = radiusTarget + self.obtainedRadiusToTarget - sp.desired_radius
else:
radiusTarget = radiusTarget + sp.desired_radius
#radiusTarget = drtT + sp.wheel_separation / 2.0 + irtT
#cp.target("radiusTarget: " + str(radiusTarget) + " (radius to Target)")
#if pact == -1 ... radiusTarget = -sp.desired_radius + sp.wheel_separation / 2.0 + self.obtainedRadiusToTarget
#if pact != -1 ... radiusTarget = sp.desired_radius + sp.wheel_separation / 2.0
radiusRobot = self.robot.linear * (1 - pacTR**2)
#cp.robot("radiusRobot: " + str(radiusRobot) + " (radius to Robot)")
#linear = hw * wander_linear + ht * target_linear + hr * robot_linear
#print "linear: ", linear
#radius = hw * wander_radius + ht * target_radius + hr * robot_radius
#print "radius: ", radius
#angular = 0.0 se raio == 0.0
vAngWander = 0.0 if radiusWander == 0.0 else hSideWander * vLinWander/radiusWander
#cp.wander("vAngWander : " + str(vAngWander) + " (angular velocity to Wander)")
vAngTarget = 0.0
if(radiusTarget != 0.0):
if(pactT == -1):
vAngTarget = hSideTarget * vLinTarget/radiusTarget * (-1)
else:
vAngTarget = hSideTarget * vLinTarget/radiusTarget * (1 + pactT)
vAngTarget = self.getHardLimited(-1.0, vAngTarget, 1.0)
#cp.target("vAngTarget : " + str(vAngTarget) + " (angular velocity to Target)")
#self.angularVelocity = (self.linearVelocity / self.desired_radius) * (1 + pactT)
#vAngRobot = 0.0 if radiusRobot == 0.0 else hSideRobot * vLinRobot/radiusRobot
#getHardLimited(self, min_value, value, max_value)
vAngRobot = (0.0 if radiusRobot == 0.0 else hSideRobot * vLinRobot/radiusRobot)
vAngRobot = self.getHardLimited(-1.0, vAngRobot, 1.0)
#cp.robot("vAngRobot : " + str(vAngRobot) + " (angular velocity to Robot)")
#angular = wander_ang + target_ang + robot_ang
#print "angular: ", angular
self.linearVelocity = (vLinFollow*hSideFollow + vLinWander*hSideWander + vLinTarget*hSideTarget + vLinRobot*hSideRobot) / (hSideFollow + hSideWander + hSideTarget + hSideRobot)
self.angularVelocity = (vAngFollow + vAngWander + vAngTarget + vAngRobot) / (hSideFollow + hSideWander + hSideTarget + hSideRobot)
print "v : ", self.linearVelocity
print "w : ", self.angularVelocity
print "r(t-1) : ", self.obtainedRadiusToTarget
print "r(t) : ", self.linearVelocity / (self.angularVelocity + 0.00001)
print "obtainedDistanceToRobot: ", self.obtainedDistanceToRobot
print ("light_factor: %.5f" %(self.light_factor))
### ["Searching","Avoiding","Following","Circumnavegating","Escorting"]
if(self.hasTarget): #Circulating and not self.hasRobot
self.status = 0
#if(pact == -1.0):
#print "pact == -1"
#self.desired_radius = self.obtainedRadiusToTarget - sp.desired_radius + sp.wheel_separation/2
#self.linearVelocity = sp.max_linear_velocity * pa
#self.linearVelocity = sp.min_linear_velocity + sp.delta_linear_velocity * pa
#elf.angularVelocity = -(self.linearVelocity / self.desired_radius) #equivale a multiplicar pelo pact, que eh -1
#else:
#print "pact != -1"
#self.desired_radius = sp.desired_radius + sp.wheel_separation / 2.0
#self.linearVelocity = sp.max_linear_velocity
#self.linearVelocity = sp.min_linear_velocity + sp.delta_linear_velocity * bic * pde
#self.angularVelocity = (self.linearVelocity / self.desired_radius) * (1 + pact)
#if(self.hasRobot):
#pactr = self.getPropAngularCoefToRobot()
#self.desired_radius = ((self.desired_radius + self.robot.linear)/2.0) * (1 - pactr)
#self.angularVelocity = (self.linearVelocity / self.desired_radius) * (1 + pactr)
#print "iap:", iap
#print "pact:", pact
#print "rconv:", rconv
#print "propAng", propAng
#print "linearVelocity", self.linearVelocity
#print "angularVelocity", self.angularVelocity
#print "pidLinear : ", self.pidLinear.update(self.obtainedRadiusToTarget)
#print "pidAngular: ", self.pidAngular.update(self.obtainedAngleToTarget)
#print "vel angular: ", self.angularVelocity
#elif(self.hasTarget and self.hasRobot): #Escorting
#self.status = 3
#self.linearVelocity = 1.0
elif(not self.hasTarget and self.hasRobot): #Avoiding
self.status = 4
#abordagem que diminui a velocidade linear para desviar
#iap = self.getInterfAngularProjected()
#self.linearVelocity = self.linearVelocity * iap
#print "robot.angular", self.robot.angular
#abordagem que circunavega o outro robo em rotacao inversa
#pactr = self.getPropAngularCoefToRobot()
#print "pactr: ", pactr
#self.desired_radius = ((self.desired_radius + self.robot.linear)/2.0) * (1 - pactr)
#if(self.robot.angular > 0.0):
#self.desired_radius += -1
#print "desired_radius: ", self.desired_radius
#self.linearVelocity = sp.max_linear_velocity
#self.angularVelocity = (self.linearVelocity / self.desired_radius) * (1 + pactr)
#print "angularVelocity: ", self.angularVelocity
else: #status == "Seeking"
self.status = 0
#self.linearVelocity = sp.linear_velocity
#decimal_random_rate = np.random.random() / 10.0
#self.angularVelocity = sp.angular_velocity * (1 + decimal_random_rate)
###------------------------------###
### Atualizacao das propriedades ###
myVelocities.angular = self.angularVelocity
myVelocities.linear = self.linearVelocity
#atualiza rotacoes e velocidades tangenciais #monitoramento
self.rightLinearVelocity = myVelocities.angular + myVelocities.linear
self.leftLinearVelocity = - 2 * myVelocities.linear - myVelocities.angular
self.rightWheelRotation = self.rightLinearVelocity / (sp.wheel_diameter * np.pi)
self.leftWheelRotation = self.leftLinearVelocity / (sp.wheel_diameter * np.pi)
#self.printAll(numRobot)
return myVelocities
def getHardLimited(self, min_value, value, max_value):
hlim = 0.0
if(value < min_value):
hlim = min_value
elif(value > max_value):
hlim = max_value
else:
hlim = value
return hlim
def process(self, now):
t = now.secs + (now.nsecs / 1E+9)
delta_time, self.time = t - self.time, t
#self.setTime(now.secs + now.nsecs / (10**9)) # segundos.milisegundos
if self.has_target and self.has_robot: #Escorting
self.status = 3
robot_dist_norm = abs(
self.robot.center.radius -
sp.desired_distance_to_robot) / sp.sensor_cone_radius
# primeiro verifica se jah estah circunavegando...
if abs(self.target.center.radius - sp.desired_radius) < 0.5:
self.linear_speed = self.linear_speed * robot_dist_norm
#pacr = -1.0
#if self.target.angular <= sp.min_ctrl_angle:
# pacr = -1.0
#elif self.target.angular >= sp.max_ctrl_angle:
# pacr = 1.0
#else:
# pacr = 2 * (self.target.angular - sp.min_ctrl_angle) / (sp.max_ctrl_angle - sp.min_ctrl_angle) - 1
#self.propAngularCoefToRobot = pacr
#pacr = prop_ang_coef(self.laser_data["robots"][0])
#self.linear_speed = 2 * sp.linear_velocity
# self.proximityCoefToRobot = self.getProximityCoefToRobot() # * self.getPropAngularCoefToRobot()
# self.linearVelocity = sp.linear_velocity + sp.linear_velocity * self.propAngularCoefToRobot * self.proximityCoefToRobot
elif self.has_target and not self.has_robot: #Circulating
self.status = 2
tcoords = LinAng()
tcoords.setAllCoords(self.target.center.getAllCoords())
radius_coef = 1 / (1 + exp(-(tcoords.linear - sp.desired_radius) /
sp.boltzmann_time_constant))
#circRadiusCoef, appRadiusCoef = self.getTransitionCoefs()
kpA = prop_ang_coef(tcoords.angular)
error_radius = sp.desired_radius - tcoords.linear
#error_radius = sp.desired_radius - self.target.linear
self.app_radius = -(tcoords.x**2 + tcoords.y**2 - sp.desired_radius
**2) / (2 * (sp.desired_radius - tcoords.y))
#appRadius = -(self.target.x**2 + self.target.y**2 - sp.desired_radius**2) / (2 * (sp.desired_radius - self.target.y))
circ_radius = sp.desired_radius + error_radius
#circRadius = sp.desired_radius + error_radius
self.performed_radius = (
1 - radius_coef) * circ_radius + radius_coef * self.app_radius
# ou radius = circ_radius + radius * (app_radius - circ_radius)
ang_vel = self.linear_speed / self.performed_radius
hl_ang_vel = boundv(sp.min_angular_velocity, ang_vel,
sp.max_angular_velocity)
hl_ang_vel += kpA * sp.delta_angular
self.linear_speed = sp.linear_velocity
self.angular_speed = boundv(sp.min_angular_velocity, hl_ang_vel,
sp.max_angular_velocity)
elif not self.has_target and self.has_robot: #Avoiding
self.status = 1
speed = (sp.linear_velocity + self.linear_speed) / 2.0
if speed > 0:
time_to_collide = abs(self.robot.center.radius -
sp.desired_distance_to_robot) / speed
if (time_to_collide < 5) and abs(
(self.angular_speed * time_to_collide) % 180) < 15:
self.linear_speed = self.linear_speed * (
0.8 - 0.1 * (5 - time_to_collide))
else: #status == "Seeking"
self.status = 0
self.linear_speed = sp.linear_velocity
decimal_random_rate = np.random.random() / 10.0
self.angular_speed = sp.angular_velocity * (1 +
decimal_random_rate)
self.linear_speed = boundv(0.0, self.linear_speed,
sp.max_linear_velocity)
self.linear_speed = self.adjust_lin_velocity(self.linear_speed)
print "[" + str(t) + "] Robo=p" + str(self.num) + " status=" + str(
self.status)
self.update()
class circum:
def __init__(self):
#Raio obtido
self.obtainedRadiusToTarget = 0.0
#Angulo Obtido em Relacao ao Target
self.obtainedAngleToTarget = 0.0
#Angulo Obtido em Relacao ao Robo
self.obtainedAngleToRobot = 0.0
#Distancia obtida
self.obtainedDistanceToRobot = 0.0
#Distancia obtida ao alien
self.obtainedDistanceToAlien = 0.0
#angulo obtido ao alien
self.obtainedAngleToAlien = 0.0
#nova dimensao
self.obtainedDistanceFromTargetToRobot = 0.0
#novo controle
self.circularCoefToTarget = 0.0
#novo controle
self.propDistCoefToRobot = 0.0
#novo controle
self.linearCoefTargetToRobot = 0.0
#novo controle
self.propAngularCoefToRobot = 0.0
#novo controle
self.linearCoefToTarget = 0.0
#self
self.proximityCoefToRobot = 0.0
#self
self.actualTime = 0.0
#self
self.previousTime = 0.0
#self
self.approachRadius = 0.0
#self
#self.vRobot = LinAng()
#localizacao x,y , com base no frame centralizado em mim
self.target = LinAng()
self.robot = LinAng()
self.alien = LinAng()
#substituida por linear velocity, angular_velocity
self.linearVelocity = sp.linear_velocity
self.angularVelocity = sp.angular_velocity
self.realLinearVelocity = 0.0
self.realAngularVelocity = 0.0
#velocidade tangencial/linear da roda direita e esquerda #m/s
self.rightLinearVelocity = 0.0
self.leftLinearVelocity = 0.0
#rotacao da roda direita e esquerda #r.p.s
self.rightWheelRotation = 0.0
self.leftWheelRotation = 0.0
#Target detectado?
self.hasTarget = False
#Robo detectado?
self.hasRobot = False
#Alien detectado?
self.hasAlien = False
#Status
self.status = 0
#pids
self.pidLinear = PID(3.0, 4.0, 1.2)
self.pidLinear.setPoint(sp.desired_radius)
self.pidAngular = PID(3.0, 4.0, 1.2)
self.pidAngular.setPoint(sp.desired_angle_to_target)
#mutable circumnavigation appRadiusCoef
self.desired_radius = sp.desired_radius
def setTime(self, time):
self.previousTime = self.actualTime
self.actualTime = time
def getPropInterFromRobot(self, target_to_robot_distance): #pifR
#gera um trapezio
#utilizada para medir a crenca na circunavegacao do robo
#com base na distancia x entre o alvo e o robo
#retorna 1.0 quando x estiver entre minima distancia e raio desejado
#retorna proporcional quando x < minima distancia
#retorna proporcional quando x > raio desejado
#retorna 0,0 quando a distancia for 0 ou maior que raio do sensor
x = target_to_robot_distance
a = 0.0
b = sp.min_distance_to_robot
c = sp.desired_radius
d = sp.sensor_cone_radius
return max(min((x - a) / (b - a), 1.0, (d - x) / (d - c)), 0.0)
def getPropDistToRobot(self): #pdtR
#gera uma valor de erro proporcional a distancia desejada ao robo
#utilizado para incrementar as velocidades linear e angular
#se obtida = desejada, pde = 1.0
#se obtida > desejada, pde = 1.xxxx
#se obtida < desejada, pde = 0.9xxx
if (self.obtainedDistanceToRobot == sp.sensor_cone_radius):
return 1.0
return (sp.sensor_cone_radius + self.obtainedDistanceToRobot) / (
sp.sensor_cone_radius + sp.desired_distance_to_robot)
def getDistanceFromTargetToRobot(self): #dfTtR
#retorna a distancia do alvo ao robo, quando houver os dois
if ((not self.hasRobot) or (not self.hasTarget)):
return 0.0
else:
return ((self.target.x - self.robot.x)**2 +
(self.target.y - self.robot.y)**2)**0.5
def getPropAngularCoefToTarget(self): #pactT
#sensor_angle, obtained_angle, desired_angle
#calcula o coeficiente proporcional angular em relacao do angulo ao Target
#envolve angulo do sensor, angulo desejado e angulo obtido
#retorna 0.0 quando angulo_obtido = angulo_desejado
#intervalo de retorno [-1.0, 1.0] *** plotar arquivo: "proportionalAngularToTarget.py"
#valor fixo
if self.target.angular <= sp.min_ctrl_angle:
#print "self.target.angular <= sp.min_ctrl_angle"
return -1.0
#apenas semantica
elif self.target.angular >= sp.max_ctrl_angle:
#print "self.target.angular >= sp.max_ctrl_angle"
return 1.0
#entao, min_ctrl_angle < self.target.angular < max_ctrl_angle
else:
#print "min_ctrl_angle < self.target.angular < max_ctrl_angle"
return 2 * (self.target.angular - sp.min_ctrl_angle) / (
sp.max_ctrl_angle - sp.min_ctrl_angle) - 1
#2 * (a - min)/(max - min)
def getPropRadialCoefToTarget(self): #prctT
#sensor_radius, obtained_radius, desired_radius
#calcula o valor do coeficiente com base no raio ao Target
#envolve raio do sensor, raio desejado e raio obtido
#retorna 0.0 quando raio_obtido = raio_desejado
#intervalo de retorno [-1.0, 1.0] *** plotar arquivo: "proportionalRadialToTarget.py"
#2017 ago 22
#if self.target.linear >= sp.desired_radius:
# #y.flat[high] = (2 * (x - dR)/(sR - dR) + 1) / 2
# #print "self.target.linear >= sp.desired_radius"
# return (2 * (self.target.linear - sp.desired_radius) / (sp.sensor_cone_radius - sp.desired_radius) + 1) / 2
#else:
# #print "self.target.linear < sp.desired_radius"
# return (self.target.linear - sp.desired_radius) / (sp.desired_radius - sp.min_distance_to_robot)
# #y.flat[low] = (x - dR)/(dR - mD)
#nova
#retorna 1.0 quando raio obtido = desejado
#retorna uma curva de tangente hiperbolica
#assintota negativa para 0
#assintota positiva para 2
#https://www.wolframalpha.com/input/?i=plot(1+%2B+(1+-+e%5E(-(x-3%2F2)*k))+%2F+(1+%2B+e%5E(-(x-3%2F2)*k))),+0+%3C+x+%3C+3,++k%3D6
d = self.obtainedRadiusToTarget - sp.desired_radius
cp.cyan("d: " + str(d))
k = sp.sensor_cone_radius / sp.min_distance_to_robot
cp.cyan("k: " + str(k))
m = math.e**(-d * k)
cp.cyan("m: " + str(m))
prctT = 1 - (1 - m) / (1 + m)
cp.cyan("prctT: " + str(prctT))
return prctT
#def getCircularCoefToTarget(self):
#calcula o coeficiente para circular o target
#quando ha o Target, retorna a media dos valores obtidos por getProportionalAngularToTarget e getProportionalRadialToTarget
#if self.target.angular == 0.0:
#return 0.0
#return (self.getPropAngularCoefToTarget() + self.getPropRadialCoefToTarget())/2.0
#def getPropAngularCoefToRobot(self):
#desativado pelo desuso 2017 08 08
#calcula o valor do coeficiente em relacao ao angulo ao robo (evitar colisao)
#envolve o angulo obtido e o angulo do sensor
#retorna valor [0.0, 1.0] *** plotar arquivo "getPropAngularCoefToRobot.py"
#abs_angle_dif = abs(self.obtainedAngleToRobot) #apenas valores positivos
#return (1 - angle_to_robot / (sp.sensor_cone_angle / 2.0))**2
def getPropAngularCoefToRobot(self): #pactR
#utilizado para desviar do robo circunavegando
#intervalo de retorno [-1.0, 1.0]
#return 2 * (self.robot.angular - sp.min_ctrl_angle) / (sp.max_ctrl_angle - sp.min_ctrl_angle) - 1
#wolfram alpha: f(x)=(1 - |x|/(3 * pi/4))^4, x = -3pi/4 to 3pi/4
#<https://www.wolframalpha.com/input/?i=f(x)%3D(1+-+%7Cx%7C%2F(3+*+pi%2F4))%5E4,+x+%3D+-3pi%2F4+to+3pi%2F4>
pactR = (1.0 - (abs(self.robot.angular) /
(sp.sensor_cone_angle / 2.0)))**6.0
return pactR
def getInterfAngularProjToRobot(self): #iaptR
#calcula o angulo projetado ao robo
#utiliza o R descrito pelo robo
#retorna 1.0, para angulo projetado = angulo detectado (interferencia maxima)
#retorna valor proporcional < 1.0
iaptR = 1.0
if (self.angularVelocity == 0.0):
return 1.0
r = self.linearVelocity / self.angularVelocity
if (abs(r) > sp.desired_distance_to_robot / 2.0):
fcos = (sp.desired_distance_to_robot / 2.0) / r
cp.cyan("cos: " + str(fcos))
f = math.acos(fcos)
cp.cyan("firstAngle(rad): " + str(fcos))
g = f * 180 / math.pi
cp.cyan("firstAngle(grades): " + str(g))
p = 90.0 - g
cp.cyan("projectedAngle (90 - g):" + str(p))
iaptR = abs(self.obtainedAngleToRobot - p) / sp.sensor_cone_angle
cp.cyan("iaptR" + str(iaptR))
return iaptR
# def getPropDistCoefToRobot(self): #pdctR
# #calcula coeficiente linear relacao a posicao do robo
# #apenas se a distancia obtida for menor do que a desejada
# #envolve a distancia obtida e a desejada em relacao ao robot
# #retorna valores no intervalo [0.0,1.0] *** plotar arquivo "getPropDistCoefToRobot"
# if(self.obtainedDistanceToRobot < sp.desired_distance_to_robot):
# return 1 - self.obtainedDistanceToRobot / sp.desired_distance_to_robot
# else:
# return 0.0
def getPropDistCoefToRobot(self): #pdctR
#calcula coeficiente linear relacao a posicao do robo
#apenas se a distancia obtida for menor do que a desejada
#envolve a distancia obtida e a desejada em relacao ao robot
#retorna valores no intervalo [0.0,1.0] *** plotar arquivo "getPropDistCoefToRobot2"
if (self.obtainedDistanceToRobot < sp.desired_distance_to_robot):
return 1 - self.obtainedDistanceToRobot / sp.desired_distance_to_robot
else:
return 0.0
# def getLinearCoefTargetToRobot(self, target, robot):
# #calcula o coeficiente linear em relacao a distancia do target ao robo detectado
# #plotar arquivo getLinearCoefTargetToRobot.py
# #distancia do target ao robot
# obtainedDistanceFromTargetToRobot = ((target.x - robot.x)**2 + (target.y - robot.y)**2)**0.5
# #atualiza atributo
# self.obtainedDistanceFromTargetToRobot = self.H(self.hasRobot and self.hasTarget, True) * obtainedDistanceFromTargetToRobot
#
# minObtainedDistance = sp.desired_radius - sp.min_distance_to_robot
# maxObtainedDistance = sp.desired_radius + sp.min_distance_to_robot
#
# if ((minObtainedDistance < obtainedDistanceFromTargetToRobot) and (obtainedDistanceFromTargetToRobot < maxObtainedDistance )):
# modularDistance = ((obtainedDistanceFromTargetToRobot - sp.desired_radius)**2)**0.5
# return 1 - modularDistance / sp.min_distance_to_robot
# else:
# return 0.0
def getProportionalAngleToTarget(self): #patT
num = sp.desired_angle_to_target - self.obtainedAngleToTarget
den = sp.sensor_cone_angle / 2
#wolfram alpha f(x)=(pi/2 - x)/( 3pi/4), x=-3pi/4 to 3pi/4
return num / den
def getProximityCoefToRobot(self): #pctR
#calcula o coeficiente de proximidade em relacao ao robo mais proximo
#envolve a distancia obtida, distancia desejada e minima distancia entre robos
#retorna [-0.5,0.5] *** plotar arquivo "getProximityCoefToRobot.py"
#quando a distancia desejada for igual obtida, retorna 0
if self.obtainedDistanceToRobot > 2 * sp.desired_distance_to_robot:
return 0.5
if self.obtainedDistanceToRobot < sp.min_distance_to_robot:
return -0.5
#senao, calcule y = (x - dD) / (2 * (dD - mD))
return (self.obtainedDistanceToRobot - sp.desired_distance_to_robot
) / (2 *
(sp.desired_distance_to_robot - sp.min_distance_to_robot))
def H(self, value, reference):
#Heaviside function
if value >= reference:
return 1.0
else:
return 0.0
def printAll(self, num_id):
#print "[", num_id, "]:: Ci:", self.interferenceCoef, "Cc:", self.conversionCoef, "Co", self.orientationCoef, "Cp", self.proximityCoefToRobot, "Ca", self.forwardCoef
print "-------- P", num_id, " ---------"
#print("Robot Real Velocities (vL,vA): %6.2f m/s, %6.2f rad/s " % (self.realLinearVelocity,self.realAngularVelocity))
#print("p3dx_mover velocities (vL,vA): %6.2f m/s, %6.2f rad/s " % (self.linearVelocity,self.angularVelocity))
#print("Velocidade Tang Direita : %6.2f m/s" % (self.rightLinearVelocity))
#print("Rotacao Roda Direita : %6.2f rad/s" % (self.rightWheelRotation))
#print("Velocidade Tang Esquerda : %6.2f m/s" % (self.leftLinearVelocity))
#print("Rotacao Roda Esquerda : %6.2f rad/s" % (self.leftWheelRotation))
#print("Target x,y : %6.2f, %6.2f " % (self.target))
#print( "Target (Raio, Angulo): %6.2f, %6.2f " % (self.obtainedRadiusToTarget, self.obtainedAngleToTarget))
#print("Raio do Sensor (Sr): %6.2f" % (sp.sensor_cone_radius))
#print("Raio desejado (dR): %6.2f" % (sp.desired_radius))
#print("Circular Coef Target aCtB: %6.2f" % (self.circularCoefToTarget))
#print("Robot x,y : %6.2f, %6.2f " % (self.robot))
#print(" Robot (Dist, Angulo) : %6.2f, %6.2f" % (self.obtainedDistanceToRobot, self.obtainedAngleToRobot))
#print("Min Distancia entre Rob(mD): %6.2f" % (sp.min_distance_to_robot))
#print("Distancia desejada (dD): %6.2f" % (sp.desired_distance_to_robot))
#print("proximityCoefToRobot : %6.2f " % (self.proximityCoefToRobot))
#print("propAngularCoefToRobot : %6.2f" % (self.propAngularCoefToRobot))
#print("propDistCoefToRobot : %6.2f" % (self.propDistCoefToRobot))
#print("Distancia Target to Robot : %6.2f" % (self.obtainedDistanceFromTargetToRobot))
#print("Ang. Coef Target to Robot : %6.2f" % (self.linearCoefTargetToRobot))
#print sp.status_msg[self.status], self.status
#print ""
print ""
#atualiza a existencia de objetos detectados
def updateDetectedObjects(self, detectedTargetDist, detectedRobotDist,
detectedAlienDist):
if detectedTargetDist.linear > 0.0:
self.hasTarget = True
self.obtainedRadiusToTarget, self.obtainedAngleToTarget = detectedTargetDist.getPolarCoords(
)
self.target.setRetCoords(detectedTargetDist.getRetCoords())
self.target.setPolarCoords(detectedTargetDist.getPolarCoords())
else:
self.hasTarget = False
#self.obtainedRadiusToTarget, self.obtainedAngleToTarget = 0.0, 0.0
self.obtainedRadiusToTarget, self.obtainedAngleToTarget = sp.sensor_cone_radius, sp.sensor_cone_angle
self.target.clear()
if detectedRobotDist.linear > 0.0:
self.hasRobot = True
self.obtainedDistanceToRobot, self.obtainedAngleToRobot = detectedRobotDist.getPolarCoords(
)
self.robot.setRetCoords(detectedRobotDist.getRetCoords())
self.robot.setPolarCoords(detectedRobotDist.getPolarCoords())
else:
self.hasRobot = False
#self.obtainedDistanceToRobot, self.obtainedAngleToRobot = 0.0, 0.0
self.obtainedDistanceToRobot, self.obtainedAngleToRobot = sp.sensor_cone_radius, sp.sensor_cone_angle
self.robot.clear()
if detectedAlienDist.linear > 0.0:
self.hasAlien = True
self.obtainedDistanceToAlien, self.obtainedAngleToAlien = detectedAlienDist.getPolarCoords(
)
self.alien.setRetCoords(detectedAlienDist.getRetCoords())
self.alien.setPolarCoords(detectedAlienDist.getPolarCoords())
else:
self.hasAlien = False
#self.obtainedDistanceToAlien, self.obtainedAngleToAlien = 0.0, 0.0
self.obtainedDistanceToAlien, self.obtainedAngleToAlien = sp.sensor_cone_radius, sp.sensor_cone_angle
self.alien.clear()
#devolve as velocidades linear e angular
def getVelocities(self, numRobot, myVelocities, targetCoord, robotCoord,
alienCoord, now):
#print "target coord", targetCoord.getVelocities()
#print "robot coord", robotCoord.getVelocities()
#print "alien coord", alienCoord.getVelocities()
self.setTime(now.secs + now.nsecs / (10**9)) # segundos.milisegundos
#print ("Tempo decorrido %4.9f" %(self.actualTime - self.previousTime))
#atualiza a velocidade real
self.realLinearVelocity, self.realAngularVelocity = myVelocities.getVelocities(
)
#atualiza a existencia dos objetos
self.updateDetectedObjects(targetCoord, robotCoord, alienCoord)
#age conforme os objetos detectados
#iaptR = self.getInterfAngularProjToRobot()
#prctT = self.getPropRadialCoefToTarget()
pactT = self.getPropAngularCoefToTarget() #[-1.0...1.0]
cp.target("pactT : " + str(pactT))
patT = self.getProportionalAngleToTarget(
) #[negativo, para angulo > 90.. positivo para angulo < 90
cp.target("patT : " + str(patT))
pacTR = self.getPropAngularCoefToRobot() #[-1.0...1.0]
cp.robot("pacTR : " + str(pacTR))
dfTtR = self.getDistanceFromTargetToRobot()
cp.robot("dfTtR : " + str(dfTtR))
pdtR = self.getPropDistToRobot() #[---, 1.0, +++]
cp.robot("pdtR : " + str(pdtR))
pifR = self.getPropInterFromRobot(
dfTtR) #trapezio [1.0 quando Dmin <= x <= Dd], [0.0 quando x > rS]
cp.robot("pifR : " + str(pifR))
pdctR = self.getPropDistCoefToRobot()
cp.robot("pdctR : " + str(pdctR))
#heaviside wandering eh 0.0 se um dos dois forem detectados
hW = (0.0 if (self.obtainedRadiusToTarget < sp.sensor_cone_radius
or self.obtainedDistanceToRobot < sp.sensor_cone_radius)
else 1.0)
cp.wander("hW: " + str(hW) + " (heaviside Wander)")
vW = sp.linear_velocity
cp.wander("vW: " + str(vW) + " (linear velocity to Wander)")
rW = sp.linear_velocity / (sp.angular_velocity *
(1 + (np.random.random() / 10.0)))
cp.wander("rW: " + str(rW) + " (radius to Wander)")
hT = (1.0
if self.obtainedRadiusToTarget < sp.sensor_cone_radius else 0.0)
cp.target("hT: " + str(hT) + " (heaviside Target)")
hR = (1.0
if self.obtainedDistanceToRobot < sp.sensor_cone_radius else 0.0)
cp.robot("hR: " + str(hR) + " (heaviside Robot)")
#vT = sp.min_linear_velocity + sp.delta_linear_velocity * patT
vT = sp.max_linear_velocity
if (pactT == -1):
vT = vT * patT
cp.target("vT: " + str(vT) + " (linear velocity to Target)")
#if pacT == -1 ... self.linearVelocity = sp.min_linear_velocity + sp.delta_linear_velocity * patT
# else self.linearVelocity = sp.min_linear_velocity + sp.delta_linear_velocity * bic * pde
#vR = sp.min_linear_velocity + sp.delta_linear_velocity * pifR**2 * pdtR
vR = sp.max_linear_velocity * (pdctR * pdtR)**pifR
cp.robot("vR: " + str(vR) + " (linear velocity to Robot)")
#irtT = (0.0 if pactT == -1 else self.obtainedRadiusToTarget)
#cp.target("irtt: " + str(irtT) + " (inverse radius to Target)")
#drtT = (sp.desired_radius if pactT == -1 else sp.desired_radius * (-1))
#cp.target("drtT: " + str(drtT) + " (desired radius to Target)")
rT = sp.wheel_separation / 2.0
if (pactT == -1):
rT = rT + self.obtainedRadiusToTarget - sp.desired_radius
else:
rT = rT + sp.desired_radius
#rT = drtT + sp.wheel_separation / 2.0 + irtT
cp.target("rT: " + str(rT) + " (radius to Target)")
#if pact == -1 ... rT = -sp.desired_radius + sp.wheel_separation / 2.0 + self.obtainedRadiusToTarget
#if pact != -1 ... rT = sp.desired_radius + sp.wheel_separation / 2.0
rR = self.robot.linear * (1 - pacTR**2)
cp.robot("rR: " + str(rR) + " (radius to Robot)")
#linear = hw * wander_linear + ht * target_linear + hr * robot_linear
#print "linear: ", linear
#radius = hw * wander_radius + ht * target_radius + hr * robot_radius
#print "radius: ", radius
#angular = 0.0 se raio == 0.0
wW = 0.0 if rW == 0.0 else hW * vW / rW
cp.wander("wW : " + str(wW) + " (angular velocity to Wander)")
wT = 0.0
if (rT != 0.0):
if (pactT == -1):
wT = hT * vT / rT * (-1)
else:
wT = hT * vT / rT * (1 + pactT)
wT = self.getHardLimited(-1.0, wT, 1.0)
cp.target("wT : " + str(wT) + " (angular velocity to Target)")
#self.angularVelocity = (self.linearVelocity / self.desired_radius) * (1 + pactT)
#wR = 0.0 if rR == 0.0 else hR * vR/rR
#getHardLimited(self, min_value, value, max_value)
wR = (0.0 if rR == 0.0 else hR * vR / rR)
wR = self.getHardLimited(-1.0, wR, 1.0)
cp.robot("wR : " + str(wR) + " (angular velocity to Robot)")
#angular = wander_ang + target_ang + robot_ang
#print "angular: ", angular
self.linearVelocity = (vW * hW + vT * hT + vR * hR) / (hW + hT + hR)
self.angularVelocity = (wW + wT + wR) / (hW + hT + hR)
print "v : ", self.linearVelocity
print "w : ", self.angularVelocity
print "r(t-1) : ", self.obtainedRadiusToTarget
print "r(t) : ", self.linearVelocity / (self.angularVelocity +
0.00001)
print "obtainedDistanceToRobot: ", self.obtainedDistanceToRobot
if (self.hasTarget): #Circulating and not self.hasRobot
self.status = 0
#if(pact == -1.0):
#print "pact == -1"
#self.desired_radius = self.obtainedRadiusToTarget - sp.desired_radius + sp.wheel_separation/2
#self.linearVelocity = sp.max_linear_velocity * pa
#self.linearVelocity = sp.min_linear_velocity + sp.delta_linear_velocity * pa
#elf.angularVelocity = -(self.linearVelocity / self.desired_radius) #equivale a multiplicar pelo pact, que eh -1
#else:
#print "pact != -1"
#self.desired_radius = sp.desired_radius + sp.wheel_separation / 2.0
#self.linearVelocity = sp.max_linear_velocity
#self.linearVelocity = sp.min_linear_velocity + sp.delta_linear_velocity * bic * pde
#self.angularVelocity = (self.linearVelocity / self.desired_radius) * (1 + pact)
#if(self.hasRobot):
#pactr = self.getPropAngularCoefToRobot()
#self.desired_radius = ((self.desired_radius + self.robot.linear)/2.0) * (1 - pactr)
#self.angularVelocity = (self.linearVelocity / self.desired_radius) * (1 + pactr)
#print "iap:", iap
#print "pact:", pact
#print "rconv:", rconv
#print "propAng", propAng
#print "linearVelocity", self.linearVelocity
#print "angularVelocity", self.angularVelocity
#print "pidLinear : ", self.pidLinear.update(self.obtainedRadiusToTarget)
#print "pidAngular: ", self.pidAngular.update(self.obtainedAngleToTarget)
#print "vel angular: ", self.angularVelocity
#elif(self.hasTarget and self.hasRobot): #Escorting
#self.status = 3
#self.linearVelocity = 1.0
elif (not self.hasTarget and self.hasRobot): #Avoiding
self.status = 4
#abordagem que diminui a velocidade linear para desviar
#iap = self.getInterfAngularProjected()
#self.linearVelocity = self.linearVelocity * iap
#print "robot.angular", self.robot.angular
#abordagem que circunavega o outro robo em rotacao inversa
#pactr = self.getPropAngularCoefToRobot()
#print "pactr: ", pactr
#self.desired_radius = ((self.desired_radius + self.robot.linear)/2.0) * (1 - pactr)
#if(self.robot.angular > 0.0):
#self.desired_radius += -1
#print "desired_radius: ", self.desired_radius
#self.linearVelocity = sp.max_linear_velocity
#self.angularVelocity = (self.linearVelocity / self.desired_radius) * (1 + pactr)
#print "angularVelocity: ", self.angularVelocity
else: #status == "Seeking"
self.status = 0
#self.linearVelocity = sp.linear_velocity
#decimal_random_rate = np.random.random() / 10.0
#self.angularVelocity = sp.angular_velocity * (1 + decimal_random_rate)
###------------------------------###
### Atualizacao das propriedades ###
myVelocities.angular = self.angularVelocity
myVelocities.linear = self.linearVelocity
#atualiza rotacoes e velocidades tangenciais #monitoramento
self.rightLinearVelocity = myVelocities.angular + myVelocities.linear
self.leftLinearVelocity = -2 * myVelocities.linear - myVelocities.angular
self.rightWheelRotation = self.rightLinearVelocity / (
sp.wheel_diameter * np.pi)
self.leftWheelRotation = self.leftLinearVelocity / (sp.wheel_diameter *
np.pi)
#self.printAll(numRobot)
return myVelocities
#def getDelimitedLinearVelocity(self, linearVelocity):
#if(linearVelocity < 0.0):
#return 0.0
#elif(linearVelocity > sp.max_linear_velocity):
#return sp.max_linear_velocity
#else:
#return linearVelocity
def getHardLimited(self, min_value, value, max_value):
hlim = 0.0
if (value < min_value):
hlim = min_value
elif (value > max_value):
hlim = max_value
else:
hlim = value
return hlim
