Robot Pepper / получить значение обнаружения препятствий - PullRequest
Новая
       9

Robot Pepper / получить значение обнаружения препятствий

0 голосов
/ 02 мая 2018

Я работаю с роботом Pepper для навигации по Python.

Как узнать значение расстояния между роботом и препятствием в разных направлениях: спереди, слева, справа и сзади?

1 Ответ

0 голосов
/ 02 мая 2018

Я давно не работал с Пеппер, но вот гист того, что я делал, когда экспериментировал с лазерами.

Насколько я помню, код заставлял робота вращаться сам по себе, регистрируя расстояния от переднего, левого и правого лазерных наборов.

Обратите внимание, что это, вероятно, для более старого API .

Я еще немного покопаюсь в документах, но это может вам помочь!

Я обновил суть с другой версией, которая показывает, как включить лазеры, используя DCM, но он читает только фронтальные значения.

Проверьте это для предыдущего кода (более ранняя версия gist), который читает каждое направление.

По запросу, вот код для новейшей сущности . 

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