Pepper lasers

PepperLasers.py

import qi
import time
import sys
import almath
from matplotlib import pyplot as plt
import math

# robotIP = "150.145.115.50"
robotIP = "194.119.214.252"
port = 9559

session = qi.Session()
print ("Connecting to " + robotIP + ":" + str(port))
session.connect("tcp://" + robotIP + ":" + str(port))
print ("Connected, starting the test")

memoryProxy = session.service("ALMemory")
motion_service  = session.service("ALMotion")

distances = []


front_values = [[],[]]
for i in range(1,16):
    # print "Processing front segment ",i
    if i<10:
        stringIndex = "0" + str(i)
    else:
        stringIndex = str(i)
    y_value = memoryProxy.getData("Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg"+stringIndex+"/X/Sensor/Value")# - 0.0562
    x_value = -memoryProxy.getData("Device/SubDeviceList/Platform/LaserSensor/Front/Horizontal/Seg"+stringIndex+"/Y/Sensor/Value")
    # point = [x_value,y_value]
    front_values[0].append(x_value)
    front_values[1].append(y_value)

left_values = [[],[]]
for i in range(1,16):
    # print "Processing front segment ",i
    if i<10:
        stringIndex = "0" + str(i)
    else:
        stringIndex = str(i)
    y_value = memoryProxy.getData("Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg"+stringIndex+"/X/Sensor/Value") #- 0.0899
    x_value = -memoryProxy.getData("Device/SubDeviceList/Platform/LaserSensor/Left/Horizontal/Seg"+stringIndex+"/Y/Sensor/Value")
    # point = [x_value,y_value]
    left_values[0].append(-y_value)
    left_values[1].append(x_value)

right_values = [[],[]]
for i in range(1,16):
    # print "Processing front segment ",i
    if i<10:
        stringIndex = "0" + str(i)
    else:
        stringIndex = str(i)
    y_value = memoryProxy.getData("Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg"+stringIndex+"/X/Sensor/Value") #- 0.0899
    x_value = -memoryProxy.getData("Device/SubDeviceList/Platform/LaserSensor/Right/Horizontal/Seg"+stringIndex+"/Y/Sensor/Value")
    # point = [x_value,y_value]
    right_values[0].append(y_value)
    right_values[1].append(-x_value)

# for x in left_values[1]:
#     x = -x
# for x in right_values[0]:
#     x = -x
plt.figure(0)
plt.plot(left_values[0],left_values[1],color="red")
# plt.figure(1)
plt.plot(front_values[0],front_values[1],color="black")
# plt.figure(2)
plt.plot(right_values[0],right_values[1],color="blue")

# plt.figure(1)
# plt.plot(left_values[0],left_values[1],color="red")
# plt.figure(2)
# plt.plot(front_values[0],front_values[1],color="black")
# plt.figure(3)
# plt.plot(right_values[0],right_values[1],color="blue")

df = [0 for i in front_values[0]]
dr = [0 for i in right_values[0]]
dl = [0 for i in left_values[0]]

for i in range(len(front_values[0])):
    # print "Processing ", i
    df[i] = front_values[0][i]*front_values[0][i] + front_values[1][i]*front_values[1][i]
    dr[i] = right_values[0][i]*right_values[0][i] + right_values[1][i]*right_values[1][i]
    dl[i] = left_values[0][i]*left_values[0][i] + left_values[1][i]*left_values[1][i]


distances = df+dr+dl
maxTotal = max(distances)

index = distances.index(maxTotal)

maxDistance = math.sqrt(maxTotal)

x_s = front_values[0] + right_values[0] + left_values[0]
y_s = front_values[1] + right_values[1] + left_values[1]
max_x = x_s[index]
max_y = y_s[index]

plt.scatter(max_x,max_y,color="green")

print index
plt.show()


theta = math.atan(max_y/max_x)


motion_service.moveTo(0.0, 0.0, -theta)