Robot Pepper / get value of Obstacle detection

Question

I work with Pepper robot for navigation with Python.

How can I get the value of distance between the robot and the obstacle in different directions: Front, Left, Right and rear?

Answer 1

I haven't worked with Pepper for a long time but here's a gist of what I did when experimenting with the lasers.

As far as I remember, the code made the robot rotate on itself, registering the distances from the front, left and right laser sets.

Note that this is probably for an older API.

I'll dig through the docs a bit more, but this might help you!

I've updated the gist with a different version which shows how to enable the lasers using DCM, but it reads only the front values.

Check this for the previous code (older revision of the gist), which reads each direction.

As requested, here's the code for the newest 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()

And here's the older gist:

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)