forked from Auxority/hackathon-team10
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Navigation.py
38 lines (31 loc) · 1.4 KB
/
Navigation.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
from naoqi import ALProxy
import numpy as np
from PIL import Image
class Navigation:
def __init__(self, pepper):
self.navigation = ALProxy("ALNavigation", pepper._ip, pepper._port)
self.motion = ALProxy("ALMotion", pepper._ip, pepper._port)
self.navigation.stopExploration()
def explore(self, radius=2.0):
self.motion.wakeUp()
error_code = self.navigation.explore(radius)
if error_code != 0:
print("Exploration failed! Error code: " + error_code)
return
path = self.navigation.saveExploration()
print("Exploration saved at path: \"" + path + "\"")
self.navigation.startLocalization()
self.navigation.navigateToInMap([0.0, 0.0, 0.0])
self.navigation.stopLocalization()
# self.visualize_map()
self.navigation.stopExploration()
def visualize_map(self):
# self.navigation.loadExploration("/home/nao/.local/share/Explorer/2022-07-05T234428.544Z.explo")
result_map = self.navigation.getMetricalMap()
map_width = result_map[1]
map_height = result_map[2]
# create a 2d image with the data above
img = np.array(result_map[4]).reshape(map_width, map_height)
img = (100 - img) * 2.55 # from 0..100 to 255..0
img = np.array(img, np.uint8)
Image.frombuffer("L", (map_width, map_height), img, "raw", "L", 0, 1).show()