Pepper lasers

PepperLasers.py

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


# robotIP = "150.145.115.50"
robotIP = "194.119.214.251"
port = 9559

session = qi.Session()
print ("Connecting to " + robotIP + ":" + str(port))
session.connect("tcp://" + robotIP + ":" + str(port))


memoryProxy = session.service("ALMemory")
motion_service  = session.service("ALMotion")
dcm_service = session.service("DCM")
t = dcm_service.getTime(0)
dcm_service.set(["Device/SubDeviceList/Platform/LaserSensor/Front/Reg/OperationMode/Actuator/Value", "Merge", [[1.0, t]]])
dcm_service.set(["Device/SubDeviceList/Platform/LaserSensor/Right/Reg/OperationMode/Actuator/Value", "Merge", [[1.0, t]]])
dcm_service.set(["Device/SubDeviceList/Platform/LaserSensor/Left/Reg/OperationMode/Actuator/Value", "Merge", [[1.0, t]]])
motion_service.setExternalCollisionProtectionEnabled("All", True)
memoryProxy = session.service("ALMemory")

theta0 = motion_service.getRobotPosition(False)[2]
data = []
speed = 0.5

print theta0

motion_service.moveToward(0.0,0.0,speed)
try:
    while memoryProxy.getData("MONITOR_RUN")>0:
        theta = motion_service.getRobotPosition(False)[2] -theta0 + 1.57
        for i in range(0,15):
            if i+1<10:
                stringIndex = "0" + str(i+1)
            else:
                stringIndex = str(i+1)

            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")

            data.append((theta+(0.523599-i*0.0698132),math.sqrt(x_value*x_value + y_value*y_value)))
except KeyboardInterrupt:
    print "Stopped"
motion_service.stopMove()
plt.figure(0)
plt.subplot(111, projection='polar')
data2 = sorted(data)

thetas = []
distances = []
for x in data2:
    thetas.append(x[0])
    distances.append(x[1])
print len(thetas)
plt.plot(thetas,distances)
plt.show()