Robot Pepper / получить значение обнаружения препятствий
Я работаю с роботом Pepper для навигации с Python.
Как я могу получить значение расстояния между роботом и препятствием в разных направлениях: спереди, слева, справа и сзади?
1 ответ
Я давно не работал с Пеппер, но вот суть того, что я делал, когда экспериментировал с лазерами.
Насколько я помню, код заставлял робота вращаться сам по себе, регистрируя расстояния от переднего, левого и правого лазерных наборов.
Обратите внимание, что это, вероятно, для более старого 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)