def setup(self):
## read config
toplevel_yaml = core.get_full_path('config/toplevel.yaml')
data = open(toplevel_yaml).read()
yamls = yaml.load(data)
rover.setup(core.get_full_path(yamls['rover']))
camera.setup(core.get_full_path(yamls['camera']))
obc.setup()
vo.setup(rover)
mapper.setup(core.get_full_path(yamls['mapper']))
dem.setup(core.get_full_path(yamls['mapper']))
self.pose = pose2d.Pose2D()
self.rate_pl = rate.Rate(0.7, name='pipeline')
## Messaging
self.sendq = Queue.Queue()
## Navigation
self.goals = Queue.Queue()
self.next_goal = None
self.wp = np.empty((0, 3))
self.flag_de = False
self.distance = 0
self.prev_pose = np.zeros(3)
self.datadir = 'log/image/{}'.format(
time.strftime("%Y-%m-%d_%H-%M-%S", time.localtime()))
os.mkdir(self.datadir)
def setup(self):
## read config
toplevel_yaml = core.get_full_path('config/toplevel.yaml')
data = open(toplevel_yaml).read()
yamls = yaml.load(data)
rover.setup(core.get_full_path(yamls['rover']))
camera.setup(core.get_full_path(yamls['camera']))
obc.setup()
vo.setup(rover)
mapper.setup(core.get_full_path(yamls['mapper']))
dem.setup(core.get_full_path(yamls['mapper']))
self.pose = pose2d.Pose2D()
self.rate_pl = rate.Rate(0.7, name='pipeline')
## Messaging
self.sendq = Queue.Queue()
## Navigation
self.goals = Queue.Queue()
self.next_goal = None
self.wp = np.empty((0, 3))
self.flag_de = False
self.distance = 0
self.prev_pose = np.zeros(3)
self.datadir = 'log/image/{}'.format(time.strftime("%Y-%m-%d_%H-%M-%S", time.localtime()))
os.mkdir(self.datadir)
def setup():
'''Execute initialization procedure
This method should not block.
'''
global pose
rover.setup(core.get_full_path('config/rover_coords.yaml'))
camera.setup(core.get_full_path('config/camera_config.yaml'))
obc.setup() #TODO make it yaml
vo.setup(rover)
mapper.setup(core.get_full_path('config/map_config.yaml'))
pose = pose2d.Pose2D()
import numpy as np
import cv2
import matplotlib.pyplot as plt
import matplotlib.patches as pch
import libviso2 as vo
from aurora.loc import rover
from aurora.loc import pose2d_ekf as pose2d
from aurora.hw import camera
from aurora.core import core
## Sample code
if __name__ == '__main__':
# load configure from files
rover.setup(core.get_full_path('config/rover_coords.yaml'))
camera.setup(core.get_full_path('config/camera_local_config.yaml'))
vo.setup(rover)
pose = pose2d.Pose2DEKF()
pose.setup(core.get_full_path('config/ekf_config.yaml'))
# perform odometry
plt.figure()
plt.hold('on')
plt.axis('equal')
pos = np.zeros((0, 3))
for i in range(1000):
frame, imL, imR = camera.get_stereo_images()
if imL is None: break
imLg = cv2.cvtColor(imL, cv2.COLOR_BGR2GRAY)
imRg = cv2.cvtColor(imR, cv2.COLOR_BGR2GRAY)
cv2.imshow('image', imL)
sys.settrace
import numpy as np
import cv2
import os
from aurora.core import core, decorator
from aurora.hw import camera
from aurora.demo import itokawa
## Sample code
if __name__ == '__main__':
# load image
camera.setup(core.get_full_path('config/camera_config.yaml'))
frame, im = camera.get_mono_image()
# detect itokawa
itokawa_mat = itokawa.detect_watanabe(im)
# display
disp = im.copy()
cv2.circle(disp, tuple([p for p in itokawa_mat]), 3, (0, 255, 0))
datadir = core.get_full_path('viz/static/img')
cv2.imwrite(os.path.join(datadir, '_images_left.png'), cv2.resize(disp, (disp.shape[1]/2, disp.shape[0]/2)))
cv2.imwrite(os.path.join(datadir, '_images_cost_map.png'), disp)
#cv2.imshow('itokawa', disp)
cv2.waitKey(-1)