问机器人胡椒/获得障碍物检测的值

EN

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

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)