def talker():
global d
global yaw
global gravidade_x
global gravidade_y
global gravidade_z
global rms, vel_linear, theta
fuzzy_autonomy.inicializaFuzzy()
pubAutonomy = rospy.Publisher('autonomy_level', Int16, queue_size=10)
rospy.Subscriber('air1/lrs36', LaserScan, laserCallback)
rospy.Subscriber('air1/odon', Odometry, odonCallback)
rospy.Subscriber('air1/twist', TwistStamped, twistCallback)
rospy.Subscriber('joy', Joy, joyCallback)
rospy.Subscriber('/myo/rms', Float32, rmsCallback)
rospy.init_node('select_autonomy_node', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
data = d
if data != 0:
erro_x = 0
scan = ajusteTanque.laserScanToPointCloud(data)
tanque = ajusteTanque.calcularPosicaoTanque(scan)
if tanque != -1:
sinal = ajusteTanque.determinarSinal(scan, tanque)
erro_x = ajusteTanque.calcularCentroSolda(sinal)
pubErro.publish(erro_x)
erro_orientacao = 0
#erroacumulado = erroacumulado + erro_x*erro_x
#print("ErroAcumulado:")
#print erroacumulado
#elapsed_time = time.time() - start_time
#print("Tempo:")
#print elapsed_time
#erro_por_tempo = math.sqrt(erroacumulado)/elapsed_time
#print("metrica:")
#print erro_por_tempo
autonomy = fuzzy_autonomy.calculateAutonomy(
rms, vel_linear, theta, erro_x)
pubAutonomy.publish(autonomy)
rate.sleep()
def callback(data):
scan = ajusteTanque.laserScanToPointCloud(data)
tanque = ajusteTanque.calcularPosicaoTanque(scan)
filtrado = filtro.aplicarFiltro(scan)
derivada = ajusteTanque.determinarDerivada(filtrado)
sinal = ajusteTanque.determinarSinal(filtrado, tanque)
# pubPointCloud.publish(scan)
# pubTanque.publish(tanque)
# pubTanque.publish(derivada)
pubPointCloud.publish(sinal)
pubTanque.publish(ajusteTanque.testecalcularCentroSolda(sinal))
# pubFiltro.publish(filtrado)
return
def listener():
global d
global yaw
global gravidade_x
global gravidade_y
global gravidade_z
global espera_joy
global autonomy
global erroacumulado
global index_sequence
# filtro.calcularFiltro()
#filtro.criarGraficos()
fuzzy.inicializaFuzzy()
# In ROS, nodes are uniquely named. If two nodes with the same
# name are launched, the previous one is kicked off. The
# anonymous=True flag means that rospy will choose a unique
# name for our 'listener' node so that multiple listeners can
# run simultaneously.
pubFiltro = rospy.Publisher('filtro', PointCloud, queue_size=10)
pubTanque = rospy.Publisher('tanque', PointCloud, queue_size=10)
pubPointCloud = rospy.Publisher('pointLRS36', PointCloud, queue_size=10)
pubVel = rospy.Publisher('air1/cmd_vel', Twist, queue_size=10) #alterado
#pubVel = rospy.Publisher('sim/cmd_vel',Twist,queue_size=10)
pubErro = rospy.Publisher('air1/erro', Float32, queue_size=10)
rospy.init_node('processamento_node', anonymous=True)
rospy.Subscriber('air1/lrs36', LaserScan, laserCallback)
rospy.Subscriber('air1/odon', Odometry, odonCallback)
rospy.Subscriber('air1/twist', TwistStamped, twistCallback)
rospy.Subscriber('joy', Joy, joyCallback)
rospy.Subscriber('autonomy_level', Int16, autonomyCallback)
rate = rospy.Rate(1000)
orie_desejada = 0
erro_x = 0
# os.system("rqt_plot /air1/erro &")
while not rospy.is_shutdown():
data = d
if data != 0:
erro_x = 0
scan = ajusteTanque.laserScanToPointCloud(data)
tanque = ajusteTanque.calcularPosicaoTanque(scan)
#filtrado = filtro.aplicarFiltro(scan)
#derivada = ajusteTanque.determinarDerivada(scan)
if tanque != -1:
# print 'a'
sinal = ajusteTanque.determinarSinal(scan, tanque)
# pubPointCloud.publish(scan)
# pubTanque.publish(tanque)
#
#pubPointCloud.publish(scan)
#pubTanque.publish(ajusteTanque.testecalcularCentroSolda(scan))
# pubFiltro.publish(filtrado)
erro_x = ajusteTanque.calcularCentroSolda(sinal)
pubErro.publish(erro_x)
erro_orientacao = 0
erroacumulado = erroacumulado + erro_x * erro_x
print("ErroAcumulado:")
print erroacumulado
elapsed_time = time.time() - start_time
print("Tempo:")
print elapsed_time
erro_por_tempo = math.sqrt(erroacumulado) / elapsed_time
print("metrica:")
print erro_por_tempo
vang, vlin = fuzzy.calculaVelocidade(erro_x, erro_orientacao,
3, gravidade_x,
gravidade_y, gravidade_z)
msg = Twist()
msg.linear.x = 0.3 * vlin
msg.angular.z = vang
if espera_joy == 1:
msg.linear.x = 0
msg.angular.z = 0
# print erro_x, erro_orientacao, msg.linear.x, msg.angular.z
pubVel.publish(msg)
else:
# print scan.points
iMin, iMax, estado = ajusteTanque.calcularLimitesSolda(scan)
#ladoRotacao = random.randint(0,1)
if autonomy == 3:
lado[index_sequence] = lado_joy
ladoRotacao = lado[index_sequence]
print("estado:")
print estado
print("autonomylevel:")
print autonomy
if gravidade_z > 0.4:
gPos = 0
angulo = -yaw
elif gravidade_z < -0.4:
gPos = 1
angulo = yaw
else:
gPos = 2
angulo = math.atan2(-gravidade_y, gravidade_x)
orie_desejada = angulo
# cruzamento em t
if estado == 0:
if autonomy == 3:
espera_joy = 1
if ladoRotacao == 2:
orie_desejada = orie_desejada - math.pi / 2 # direita
elif ladoRotacao == 1:
orie_desejada = orie_desejada + math.pi / 2 # esquerda
# reto ou curva a direita
if estado == 1:
if autonomy == 3:
espera_joy = 1
if ladoRotacao == 2:
orie_desejada = orie_desejada - math.pi / 2
# reto ou curva a esquerda
if estado == 2:
if autonomy == 3:
espera_joy = 1
if ladoRotacao == 1:
orie_desejada = orie_desejada + math.pi / 2
index_sequence += 1
if index_sequence >= len(lado):
index_sequence = 0
if orie_desejada > math.pi:
orie_desejada = orie_desejada - 2 * math.pi
elif orie_desejada < -1 * math.pi:
orie_desejada = orie_desejada + 2 * math.pi
erro_orientacao = orie_desejada - angulo
if erro_orientacao > math.pi:
erro_orientacao = erro_orientacao - 2 * math.pi
elif erro_orientacao < -1 * math.pi:
erro_orientacao = erro_orientacao + 2 * math.pi
vang = 0
vlin = 0
# print orie_desejada,yaw,erro_orientacao
while math.fabs(erro_orientacao) > 0.05:
vang, vlin = fuzzy.calculaVelocidade(
erro_x, erro_orientacao, estado, gravidade_x,
gravidade_y, gravidade_z)
pubErro.publish(erro_x)
# print erro_orientacao
# pubErro.publish(math.fabs(erro_orientacao))
msg = Twist()
msg.angular.z = 1.25 * vang
msg.linear.x = 0.1285 * vlin
if espera_joy == 1:
msg.linear.x = 0
msg.angular.z = 0
# print erro_x, erro_orientacao, msg.linear.x, msg.angular.z
pubVel.publish(msg)
if gPos == 0:
erro_orientacao = orie_desejada - (-yaw)
elif gPos == 1:
erro_orientacao = orie_desejada - yaw
else:
erro_orientacao = orie_desejada - math.atan2(
-gravidade_y, gravidade_x)
if erro_orientacao > math.pi:
erro_orientacao = erro_orientacao - 2 * math.pi
elif erro_orientacao < -1 * math.pi:
erro_orientacao = erro_orientacao + 2 * math.pi
rate.sleep()
scan = ajusteTanque.laserScanToPointCloud(d)
iMax, iMin, tipoSolda = ajusteTanque.calcularLimitesSolda(scan)
# pubPointCloud.publish(scan)
# pubTanque.publish(ajusteTanque.testecalcularCentroSolda(scan))
# print tipoSolda
while tipoSolda != 3:
msg = Twist()
msg.linear.x = 0.01 * vlin
msg.angular.z = vang
if espera_joy == 1:
msg.linear.x = 0
msg.angular.z = 0
pubVel.publish(msg)
# print ajusteTanque.calcularLimitesSolda(scan)
scan = ajusteTanque.laserScanToPointCloud(d)
iMax, iMin, tipoSolda = ajusteTanque.calcularLimitesSolda(
scan)
rate.sleep()
print estado
rate.sleep()
def talker():
global d
global yaw
global gravidade_x
global gravidade_y
global gravidade_z
global rms, vel_linear, theta
global joyX, joyZ, autX, autZ
global autonomy_level, autonomy_level_int, autonomy_level_int_ant
global d_roll, d_pitch, d_yaw
global iniciar_dados
global vetor_dados
global start_time
global run_once
global vetor_autonomy
global transicoes
fuzzy_autonomy.inicializaFuzzy()
pubAutonomy = rospy.Publisher('autonomy_level', Float32, queue_size=10)
pubVel = rospy.Publisher('air1/cmd_vel', Twist, queue_size=10)
pubHap = rospy.Publisher('myo_raw/vibrate', UInt8, queue_size=10)
rospy.Subscriber('joy/cmd_vel', Twist, joyvelCallback)
rospy.Subscriber('sim/cmd_vel', Twist, simvelCallback)
rospy.Subscriber('air1/lrs36', LaserScan, laserCallback)
rospy.Subscriber('air1/odon', Odometry, odonCallback)
rospy.Subscriber('air1/twist', TwistStamped, twistCallback)
rospy.Subscriber('joy', Joy, joyCallback)
rospy.Subscriber('joy/iniciar_dados', Int16, dadosCallback)
rospy.Subscriber('/myo/rms', Float32, rmsCallback)
rospy.Subscriber('/myo/delta_ang', Vector3, dangCallback)
#rospy.Subscriber('/myo_raw/myo_gest', Vector3, gestCallback)
rospy.init_node('select_autonomy_node', anonymous=True)
rate = rospy.Rate(100) # hz
while not rospy.is_shutdown():
data = d
if data != 0:
erro_x = 0
scan = ajusteTanque.laserScanToPointCloud(data)
tanque = ajusteTanque.calcularPosicaoTanque(scan)
if tanque != -1:
sinal = ajusteTanque.determinarSinal(scan, tanque)
erro_x = ajusteTanque.calcularCentroSolda(sinal)
erro_orientacao = 0
if iniciar_dados == 1:
#print "Erro:", erro_x
vetor_dados.append(erro_x)
vetor_autonomy.append(autonomy_level)
if run_once == 0:
print "Iniciando medicao..."
start_time = time.time()
run_once = 1
if iniciar_dados == 0 and run_once == 1:
rms_vetor = sqrt(mean(square(vetor_dados)))
elapsed_time = time.time() - start_time
#print "Erro RMS:", rms_vetor
print "Tempo transcorrido:", elapsed_time
print "Transicoes:", transicoes
#print "Vetor dos erros:", vetor_dados
print "VetorFiltrado = ", filter(lambda a: a != -1,
vetor_dados)
print '\n'
print "VetorAutonomia = ", vetor_autonomy
vetor_dados = []
run_once = 0
autonomy_level = fuzzy_autonomy.calculateAutonomy(
rms, theta, erro_x, d_roll)
#autonomy_level = 4 #forcando o nivel de autonomia estatico
print "Nivel de autonomia:", autonomy_level
pubAutonomy.publish(autonomy_level)
msg_cmd_vel = Twist()
if joyX != 0:
joyX = joyX - d_pitch * 0.1 # um peso dos angulos do controle no valor da velocidade, pitch para cima positivo
if joyZ != 0:
joyZ = joyZ + d_roll * 0.1 # roll sentido horario positivo
if autonomy_level <= 1: # modo manual
msg_cmd_vel.linear.x = joyX # velocidade totalmente pelo controle
msg_cmd_vel.angular.z = joyZ
elif autonomy_level > 1 and autonomy_level <= 2: # modo compartilhado
msg_cmd_vel.linear.x = (autonomy_level - 1) * autX + (
2 - autonomy_level
) * joyX # uma parte da velocidade eh do controle e outra do modo autonomo
msg_cmd_vel.angular.z = (autonomy_level -
1) * autZ + (2 - autonomy_level) * joyZ
elif autonomy_level > 2 and autonomy_level <= 3: # modo supervisorio
msg_cmd_vel.linear.x = joyX
msg_cmd_vel.angular.z = autZ
elif autonomy_level > 3: # modo autonomo
msg_cmd_vel.linear.x = autX
msg_cmd_vel.angular.z = autZ
#limitadores de velocidade
if msg_cmd_vel.linear.x > 1.0:
msg_cmd_vel.linear.x = 1.0
if msg_cmd_vel.angular.z > 2.5:
msg_cmd_vel.angular.z = 2.5
pubVel.publish(msg_cmd_vel)
autonomy_level_int_ant = autonomy_level_int
# vibracao de acordo com o nivel de autonomia
if autonomy_level <= 1:
autonomy_level_int = 1
elif autonomy_level > 1 and autonomy_level <= 2:
autonomy_level_int = 2
elif autonomy_level > 2 and autonomy_level <= 3:
autonomy_level_int = 3
elif autonomy_level > 3:
autonomy_level_int = 4
if autonomy_level_int != autonomy_level_int_ant:
transicoes = transicoes + 1
# pubHap.publish(2)
#elif autonomy_level_int < autonomy_level_int_ant:
# pubHap.publish(1)
#else:
# pubHap.publish(0)
rate.sleep()