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()
def put(self):
args = parser.parse_args()
records.RECORDS[self.uri] = {
'timestamp': float(args['timestamp']),
'data': core.get_full_path('{}.png'.format(self.uri.replace('/', '_'))),
}
f = open(records.RECORDS[self.uri]['data'], 'w')
f.write(args['data'].decode('base64'))
f.close()
return records.RECORDS[self.uri]
def put(self):
args = parser.parse_args()
records.RECORDS[self.uri] = {
'timestamp':
float(args['timestamp']),
'data':
core.get_full_path('{}.png'.format(self.uri.replace('/', '_'))),
}
f = open(records.RECORDS[self.uri]['data'], 'w')
f.write(args['data'].decode('base64'))
f.close()
return records.RECORDS[self.uri]
def setup(yamlfile):
'''load config from yaml'''
data = open(yamlfile).read()
config = yaml.load(data)
global ranges, resolution, dpm, grid_size, max_disparity
ranges = np.array(config['elvmap']['ranges'], dtype=np.float)
resolution = np.array(config['elvmap']['resolution'], dtype=np.float)
dpm = config['elvmap']['dpm']
grid_size = config['elvmap']['lvd']['grid_size']
max_disparity = config['elvmap']['lvd']['max_disparity']
rover.setup(core.get_full_path(config['rover_yaml']))
dem_impl.setup(dem, rover)
def setup(yamlfile):
'''load config from yaml'''
data = open(yamlfile).read()
config = yaml.load(data)
global ranges, resolution, dpm, grid_size, max_disparity
ranges = np.array(config['elvmap']['ranges'], dtype=np.float)
resolution = np.array(config['elvmap']['resolution'], dtype=np.float)
dpm = config['elvmap']['dpm']
grid_size = config['elvmap']['lvd']['grid_size']
max_disparity = config['elvmap']['lvd']['max_disparity']
rover.setup(core.get_full_path(config['rover_yaml']))
dem_impl.setup(dem, rover)
def setup(yamlfile):
'''Load config from yaml'''
# Rover settings
data = open(yamlfile).read()
rconfig = yaml.load(data)
global cTi, iTc, bTc, cTb, bTi, iTb, cTr, rTc
iTc = tfm.euler_matrix(pi / 2 + deg2rad(rconfig['tilt']), 0, pi / 2,
'rxyz')
cTi = inv(iTc)
bTc = tfm.translation_matrix(
[0.20, rconfig['baseline'] / 2, rconfig['camera_height']])
cTb = inv(bTc)
bTi = np.dot(bTc, cTi)
iTb = inv(bTi)
rTc = tfm.translation_matrix([0, rconfig['baseline'], 0])
cTr = inv(rTc)
sTc = tfm.translation_matrix([0, rconfig['baseline'] / 2.0, 0])
cTs = inv(sTc)
global width, length
width = rconfig['width']
length = rconfig['length']
# Camera settings
data = open(core.get_full_path(rconfig['camera_yaml'])).read()
cconfig = yaml.load(data)
global baseline, focal_length
baseline = rconfig['baseline']
focal_length = cconfig['cameraL']['f']
global KL, KR, Q
KL = np.array([[cconfig['cameraL']['f'], 0, cconfig['cameraL']['uv0'][0]],
[0, cconfig['cameraL']['f'], cconfig['cameraL']['uv0'][1]],
[0, 0, 1]])
KR = np.array([[cconfig['cameraR']['f'], 0, cconfig['cameraR']['uv0'][0]],
[0, cconfig['cameraR']['f'], cconfig['cameraR']['uv0'][1]],
[0, 0, 1]])
Q = np.array([[1, 0, 0, -cconfig['cameraL']['uv0'][0]],
[0, 1, 0, -cconfig['cameraL']['uv0'][1]],
[0, 0, 0, focal_length], [0, 0, 1.0 / baseline, 0]])
def setup(self):
self.r = rate.Rate(10, name="logger")
self.cnt = 0
adc_yaml = core.get_full_path('config/logger/adc_logger.yaml')
data = open(adc_yaml).read()
logging.config.dictConfig(yaml.load(data))
self.log_adc = logging.getLogger('logger_adc')
self.log_dpc = logging.getLogger('logger_dynpick')
self.log_cms = logging.getLogger('logger_compass')
self.adc = daemons.ADCDaemon(hz=10)
self.dpc = daemons.DynPickDaemon(hz=10)
self.cms = daemons.CompassDaemon(hz=1)
self.adc.start()
self.dpc.start()
self.cms.start()
pass
def setup(yamlfile):
'''Load config from yaml'''
# Rover settings
data = open(yamlfile).read()
rconfig = yaml.load(data)
global cTi, iTc, bTc, cTb, bTi, iTb, cTr, rTc
iTc = tfm.euler_matrix(pi / 2 + deg2rad(rconfig['tilt']), 0, pi / 2, 'rxyz')
cTi = inv(iTc)
bTc = tfm.translation_matrix([0.20, rconfig['baseline'] / 2, rconfig['camera_height']])
cTb = inv(bTc)
bTi = np.dot(bTc, cTi)
iTb = inv(bTi)
rTc = tfm.translation_matrix([0, rconfig['baseline'], 0])
cTr = inv(rTc)
sTc = tfm.translation_matrix([0, rconfig['baseline'] / 2.0, 0])
cTs = inv(sTc)
global width, length
width = rconfig['width']
length = rconfig['length']
# Camera settings
data = open(core.get_full_path(rconfig['camera_yaml'])).read()
cconfig = yaml.load(data)
global baseline, focal_length
baseline = rconfig['baseline']
focal_length = cconfig['cameraL']['f']
global KL, KR, Q
KL = np.array([[cconfig['cameraL']['f'], 0, cconfig['cameraL']['uv0'][0]],
[0, cconfig['cameraL']['f'], cconfig['cameraL']['uv0'][1]],
[0, 0, 1]])
KR = np.array([[cconfig['cameraR']['f'], 0, cconfig['cameraR']['uv0'][0]],
[0, cconfig['cameraR']['f'], cconfig['cameraR']['uv0'][1]],
[0, 0, 1]])
Q = np.array([[1, 0, 0, -cconfig['cameraL']['uv0'][0]],
[0, 1, 0, -cconfig['cameraL']['uv0'][1]],
[0, 0, 0, focal_length],
[0, 0, 1.0/baseline, 0]])
def setup(self):
self.r = rate.Rate(10, name="logger")
self.cnt = 0
adc_yaml = core.get_full_path('config/logger/adc_logger.yaml')
data = open(adc_yaml).read()
logging.config.dictConfig(yaml.load(data))
self.log_adc = logging.getLogger('logger_adc')
self.log_dpc = logging.getLogger('logger_dynpick')
self.log_cms = logging.getLogger('logger_compass')
self.adc = daemons.ADCDaemon(hz=10)
self.dpc = daemons.DynPickDaemon(hz=10)
self.cms = daemons.CompassDaemon(hz=1)
self.adc.start()
self.dpc.start()
self.cms.start()
pass
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)
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)
try:
imL = qimL.get_nowait()
imR = qimR.get_nowait()
except:
imL, imR = None, None
return FRAME, imL, imR
def rectify_stereo(imL, imR):
''' adjust epipolar lines with homography matrices '''
h, w = imL.shape[:2]
rimL = cv2.warpPerspective(imL, HL, (w, h))
rimR = cv2.warpPerspective(imR, HR, (w, h))
return rimL, rimR
## Sample code
if __name__ == '__main__':
setup(core.get_full_path('config/camera_local_config.yaml'))
frame, imL = get_mono_image()
frame, imL, imR = get_stereo_images()
imL, imR = rectify_stereo(imL, imR)
cv2.imshow('image', imL)
cv2.waitKey(0)
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)
def lvd(imD):
imDEM = dem_impl.lvd(imD)
imDEM[imDEM == 0] = -1000
#return cv2.flip(cv2.flip(imDEM.T, 0), 1) # v: forward u: left
return imDEM.T
## Sample code
if __name__ == '__main__':
import os
from aurora.hw import camera
from aurora.geom import dense_stereo
camera.setup(core.get_full_path('config/camera_local_config.yaml'))
setup(core.get_full_path('config/map_config.yaml'))
# load image
frame = 0
frame, imL, imR = camera.get_stereo_images()
#imL, imR = camera.rectify_stereo(imL, imR)
cv2.imshow("Left", imL)
imLg = cv2.cvtColor(imL, cv2.COLOR_BGR2GRAY)
imRg = cv2.cvtColor(imR, cv2.COLOR_BGR2GRAY)
imD = dense_stereo.disparity(imLg, imRg)
imD = cv2.medianBlur(imD, 5)
imD = np.array(imD / 16., dtype=np.float)
imD_mask = np.zeros(imD.shape, dtype=np.uint8)
def worker(self):
stamp = time.time()
## get image
frame, imL, imR = camera.get_stereo_images()
if imL is None or imR is None:
logger.error("No image found")
self.rate_pl.sleep()
return
## save image
cv2.imwrite('{}/L{:06d}.jpg'.format(self.datadir, frame), imL)
cv2.imwrite('{}/R{:06d}.jpg'.format(self.datadir, frame), imR)
## rectify, grayscale
imL, imR = camera.rectify_stereo(imL, imR)
imLg = cv2.cvtColor(imL, cv2.COLOR_BGR2GRAY)
imRg = cv2.cvtColor(imR, cv2.COLOR_BGR2GRAY)
## compute odometry
pTc = vo.update_stereo(imLg, imRg)
p = self.pose.update_from_matrix(pTc)
#print 'INFO(nav): X={:.2f}, Y={:.2f}, THETA={:.1f}'.format(p[0], p[1], math.degrees(p[2]))
#self.sendq.put('xyh {:.3f} {:.3f} {:.3f}'.format(p[0], p[1], p[2]))
self.distance += np.linalg.norm(self.prev_pose[:2] - p[:2])
self.prev_pose = p
self.sendq.put('xyhd {:.3f} {:.3f} {:.3f} {:.3f}'.format(p[0], p[1], p[2], self.distance))
# stereo
imD = dense_stereo.disparity(imLg, imRg)
imD = np.array(imD / 16., dtype=np.float)
imD[imD < 0] = 0
#print np.unique(imD.astype(np.float32))
_imXYZ = dense_stereo.reproject(imD.astype(np.float32), rover.Q.astype(np.float32))
_imXYZ[_imXYZ[:, :, 2] < 0] = 0
_imXYZ[_imXYZ[:, :, 2] > 10] = 0
X = np.resize(_imXYZ, (_imXYZ.shape[0] * _imXYZ.shape[1], 3)).T
Y = tfm.transformp(X, rover.bTi)
imXYZ = np.resize(Y.T, _imXYZ.shape)
#print imXYZ
#print np.unique(imXYZ[:, :, 2])
#print np.amin(imXYZ[:, :, 2]), np.amax(imXYZ[:, :, 2])
rocks = 255 * np.array(imXYZ[:, :, 2] > 0.2, dtype=np.uint8)
rocks[imD == 0] = 0
# rocks
#rocks = 255 * np.array(imLg > 200, dtype=np.uint8)
kern = np.ones((15, 15), dtype=np.uint8)
rocks = cv2.dilate(cv2.erode(rocks, kern), kern)
rocks3 = np.zeros((rocks.shape[0], rocks.shape[1], 3), dtype=np.uint8)
rocks3[rocks == 0] = 1
rocks3[:, :, 2] = rocks
#rocks3[rocks == 0] = 1
# dem
imD_mask = np.zeros(imD.shape, dtype=np.uint8)
imD_mask[10:-10, 10:-10] = 1;
cv2.fillPoly(imD_mask, [np.array([[90, 470], [10, 70], [0, 70], [0, 470]], dtype=np.int32)], 0, 8)
imD *= imD_mask
imDEM = dem.lvd(imD)
imDEM3 = np.zeros((imDEM.shape[0], imDEM.shape[1], 3))
imDEM3[:, :, 2] = (imDEM + 3) * 255 / 6
## update map
mapmask = np.zeros(imL.shape, dtype=np.uint8)
mapmask[200:400, 120:520, :] = 1
mapper.vizmap.add_image(imL * mapmask, p)
mapper.hzdmap.add_image(rocks3 * mapmask, p)
mapper.elvmap.add_topview_image(imDEM3, p)
print "==================="
logger.debug('frame {} stamp {}'.format(frame, stamp))
logger.debug('pose={}'.format(p))
logger.debug('distance={}'.format(self.distance))
print "self.next_goal", self.next_goal
print "self.goals", self.goals.queue
#print "self.wp", self.wp
## planning
if self.next_goal is None:
try:
self.next_goal = self.goals.get_nowait()
except:
self.next_goal = None
if self.next_goal is not None:
if self.get_distance(self.next_goal, p[:2]) < 0.5:
self.sendq.put('! *** Goal reached *** ')
self.sendto_obc(['f',])
self.wp = np.empty((0, 3))
self.next_goal = None
if self.wp.shape[0] == 0 and self.next_goal is not None:
# generate waypoints in rover relative coordinates
self.origin = p
goal_rel = np.dot(np.linalg.inv(self.pose.matrix_from_pose(self.origin)), self.pose.matrix_from_pose(self.next_goal))
goal_rel = (goal_rel[0, 3], goal_rel[1, 3], 0)
self.wp = local_planner.compute_waypoints(mapper.hzdmap, (0, 0, 0), goal_rel)
if frame % 1 == 0 and self.wp.shape[0] > 0:
# compute pose of next waypoint in local frame
wp_rel = np.dot(np.linalg.inv(self.pose.matrix_from_pose(self.origin)), self.pose.matrix_from_pose(self.wp[0, :]))
if self.get_distance(self.wp[0, :], np.zeros(3)) < 0.5:
self.wp = np.delete(self.wp, 0, axis=0)
# generate command
if self.wp.shape[0] > 0:
next_wp_rel = self.wp[0, :].ravel()
oTp = np.dot(np.linalg.inv(self.pose.matrix_from_pose(self.origin)), self.pose.matrix_from_pose(p))
p_rel = self.pose.pose_from_matrix(oTp)
#print next_wp_rel
#print p_rel
cmd_arc = self.get_distance(next_wp_rel, p_rel)
cmd_theta = -math.degrees(np.arctan2(next_wp_rel[1] - p_rel[1], next_wp_rel[0] - p_rel[0]))
logger.debug('R={:.2f} THETA={:.2f}'.format(cmd_arc, cmd_theta))
cmd_list = []
if abs(cmd_theta) > 165:
# back
obc.set_turn_mode(False)
cmd_list.append('d{:.2f}'.format(-cmd_arc))
elif abs(cmd_theta) > 90:
# don't handle backwards waypoint. shutdown planning
obc.set_turn_mode(False)
cmd_list.append('f')
logger.warn(' *** AKI never goes back! (Planning canceled) ***')
self.next_goal = None
elif abs(cmd_theta) > 45:
# back step
obc.set_turn_mode(False)
obc.set_steer_angle(0)
#cmd_list.append('d{:.2f}'.format(-cmd_arc))
cmd_list.append('d{:.2f}'.format(-1))
else: #if abs(cmd_theta) <= 45:
# steering drive
obc.set_turn_mode(False)
obc.set_steer_angle(cmd_theta)
cmd_list.append('d{:.2f}'.format(cmd_arc))
'''
else:
# Inspot turn
obc.set_turn_mode(True)
cmd_list.append('r{:.2f}'.format(cmd_theta))
'''
self.sendto_obc(cmd_list)
## save images
if frame % 1 == 0:
topmap = mapper.vizmap.get_map(trajectory=True, grid=True, centered=True)
cv2.circle(topmap, (topmap.shape[1]/2, topmap.shape[0]/2), 25, (140, 20, 130), 4)
elvmap = mapper.elvmap.get_map(trajectory=True, grid=False, centered=True)
#print np.unique(elvmap)
hzdmap = mapper.hzdmap.get_map(trajectory=True, grid=True, centered=True)
ovlmap = topmap.copy()
ovlimL = imL.copy()
ovlmap[hzdmap[:,:,2] > 200] = hzdmap[hzdmap[:,:,2]>200]
# draw waypoints
if self.wp.shape[0] > 0:
p0 = mapper.vizmap.get_pose_pix(np.zeros(3))
a0 = (imL.shape[1] / 2, 1.5 * imL.shape[0])
wTo = self.pose.matrix_from_pose(self.origin)
wTr = self.pose.matrix_from_pose(self.pose.update((0, 0, 0)))
for i in range(self.wp.shape[0]):
oTwp = self.pose.matrix_from_pose(self.wp[i])
rTwp = np.dot(np.dot(np.linalg.inv(wTr), wTo), oTwp)
wp_r = self.pose.pose_from_matrix(rTwp)
# topview
p = mapper.vizmap.get_pose_pix(wp_r)
cv2.line(ovlmap, (int(p0[0]), int(p0[1])), (int(p[0]), int(p[1])), (200, 0, 0), 3)
cv2.circle(ovlmap, (int(p[0]), int(p[1])), 5, (0, 0, 255), -1)
p0 = p
# left
pp = np.array([[wp_r[0]], [wp_r[1]], [0]])
if wp_r[0] > 0.5:
a = tfm.projectp(pp, rover.KL, rover.iTb)
cv2.line(ovlimL, (int(a0[0]), int(a0[1])), (int(a[0]), int(a[1])), (200, 0, 0), 6)
cv2.circle(ovlimL, (int(a[0]), int(a[1])), 8, (0, 0, 255), -1)
a0 = a
for g in self.goals.queue:
wTg = self.pose.matrix_from_pose(g)
rTg = np.dot(np.linalg.inv(wTr), wTg)
g_rel = self.pose.pose_from_matrix(rTg)
p = mapper.vizmap.get_pose_pix(g_rel)
cv2.circle(ovlmap, (int(p[0]), int(p[1])), 5, (200, 0, 255), -1)
datadir = core.get_full_path('viz/static/img')
cv2.imwrite(os.path.join(datadir, '_images_left.png'), ovlimL)
cv2.imwrite(os.path.join(datadir, '_images_visual_map.png'), cv2.flip(cv2.flip(ovlmap, 1), 0))
cv2.imwrite(os.path.join(datadir, '_images_disparity.png'), cv2.resize(255 * plt.cm.jet(imD.astype(np.uint8)), (320, 240)))
cv2.imwrite(os.path.join(datadir, '_images_elev_map.png'), cv2.flip(cv2.flip(elvmap[:, :, 2].astype(np.uint8), 1), 0))
cv2.imwrite(os.path.join(datadir, '_images_hazard_map.png'), cv2.flip(cv2.flip(hzdmap[:, :, 2].astype(np.uint8), 1), 0))
#cv2.imwrite(os.path.join(datadir, '_images_elev_map.png'), cv2.flip(cv2.flip(255 * plt.cm.jet(elvmap[:, :, 2].astype(np.uint8)), 1), 0))
#cv2.imwrite(os.path.join(datadir, '_images_elev_map.png'),
#cv2.flip(cv2.transpose(cv2.flip(255 * plt.cm.jet(imDEM.astype(np.uint8)), 1)), 1))
if frame % 15 == 0:
self.wp = np.empty((0, 3))
self.rate_pl.sleep()
def loop():
'''Main loop for processing new images'''
goal = None
wp = np.empty((0, 3))
rate_pl = rate.Rate(0.7, name='pipeline')
while not term_flag:
stamp = time.time()
# get image
frame, imL, imR = camera.get_stereo_images()
if imL is None or imR is None:
print 'ERROR(cam): Failed to get image'
rate_pl.sleep()
continue
cv2.imwrite('/tmp/L{:05d}.jpg'.format(frame), imL)
cv2.imwrite('/tmp/R{:05d}.jpg'.format(frame), imR)
#imL = grey_world(imL)
#imR = grey_world(imR)
imL, imR = camera.rectify_stereo(imL, imR)
imLg = cv2.cvtColor(imL, cv2.COLOR_BGR2GRAY)
imRg = cv2.cvtColor(imR, cv2.COLOR_BGR2GRAY)
# compute odometry
pTc = vo.update_stereo(imLg, imRg)
p = pose.update_from_matrix(pTc)
print 'INFO(nav): X={:.2f}, Y={:.2f}, THETA={:.1f}'.format(
p[0], p[1], math.degrees(p[2]))
if goal is not None:
print ' ----> X={:.2f}, Y={:.2f}'.format(
goal[0], goal[1])
# detect obstacles
rockmask = rockdetect.from_sand(imL)
#rockmask = rockdetect.from_grass_lpf(imL)
#rockmask2 = rockdetect.from_grass_b(imL)
#rockmask = np.array(np.logical_or(rockmask, rockmask2) * 255, dtype=np.uint8)
#rockmask = cv2.medianBlur(rockmask, 21)
rocks = np.zeros(imL.shape, dtype=np.uint8)
rocks[rocks == 0] = 1
rocks[:, :, 2] = rockmask
# stereo
imD = dense_stereo.disparity(imLg, imRg)
imD = np.array(imD / 16., dtype=np.float)
#shadowmask = np.array(imLg < 60, dtype=np.uint8) * 1
#shadowmask[:3*shadowmask.shape[0]/4, :] = 0
# update map
mapmask = np.zeros(imL.shape, dtype=np.uint8)
mapmask[200:400, 120:520, :] = 1
#mapmask[shadowmask > 0] = [0, 0, 0]
mapper.vizmap.add_image(imL * mapmask, p)
mapper.hzdmap.add_image(rocks * mapmask, p)
# fetch new goal from queue
try:
g = GOALS.get_nowait()
goal = g
except:
pass
# goal reached?
if goal is not None:
if abs(goal[0] - p[0]) < 0.2 and abs(goal[1] - p[1]) < 0.2:
print '!!!!!!!!!!!!!!!!!!!!'
print '!! Goal reached !!'
print '!!!!!!!!!!!!!!!!!!!!'
cmd_list = []
cmd_list.append('f')
obc.send_cmd(cmd_list)
wp = np.empty((0, 3))
goal = None
if frame % 5 == 0:
if goal is not None:
# generate waypoints in rover coordinates
goal_rel = np.dot(np.linalg.inv(pose.matrix_from_pose(p)),
pose.matrix_from_pose(goal))
goal_rel = (goal_rel[0, 3], goal_rel[1, 3], 0)
wp = local_planner.compute_waypoints(mapper.hzdmap, (0, 0, 0),
goal_rel)
# send commands
if wp.shape[0] > 1:
cmd_arc = np.linalg.norm(wp[1, :2] - wp[0, :2])
cmd_theta = -math.degrees(
np.arctan2(wp[1, 1] - wp[0, 1], wp[1, 0] - wp[0, 0]))
print 'INFO(path): R={:.2f} THETA={:.2f}'.format(
cmd_arc, cmd_theta)
cmd_list = []
if abs(cmd_theta) <= 45:
# steering drive
obc.set_turn_mode(False)
obc.set_steer_angle(cmd_theta)
#cmd_list.append('s{:.2f}'.format(cmd_theta))
cmd_list.append('d{:.2f}'.format(cmd_arc))
elif abs(cmd_theta) > 165:
# back
obc.set_turn_mode(False)
cmd_list.append('d{:.2f}'.format(-cmd_arc))
else:
# Inspot turn
obc.set_turn_mode(True)
cmd_list.append('r{:.2f}'.format(cmd_theta))
obc.send_cmd(cmd_list)
else:
cmd_list = []
cmd_list.append('f')
obc.send_cmd(cmd_list)
goal = None
# detect itokawa
#if frame % 5 == 0:
if False: #goal is None:
itokawa_mat = itokawa.detect_watanabe(imL)
print itokawa_mat
if itokawa_mat[0] > 0:
cv2.circle(imL, tuple([p for p in itokawa_mat]), 20,
(0, 255, 0), 3)
ang = (itokawa_mat[0] - 240.) / 240. * (50. / 180 * math.pi)
p_r = (2., 2 * math.tan(ang), 0)
print 'rel target: ', p_r
rTg = pose.matrix_from_pose(p_r)
wTr = pose.matrix_from_pose(pose.update((0, 0, 0)))
wTg = np.dot(wTr, rTg)
pw = (wTg[0, 3], wTg[1, 3], 0)
set_new_goal(pw, clear_all=True)
print '################### ITOKAWA DETECTED ##########################'
print 'itokawa_pos', itokawa_mat
print 'itokawa_ang', ang
### Generate images ###
#if True:
if frame % 5 == 0:
# topview map
topmap = mapper.vizmap.get_map(trajectory=True,
grid=True,
centered=True)
cv2.circle(topmap, (topmap.shape[1] / 2, topmap.shape[0] / 2), 25,
(140, 20, 130), 4)
# topview obstacle map
hzdmap = mapper.hzdmap.get_map(trajectory=False,
grid=False,
centered=True)
ovlmap = topmap.copy()
ovlmap[hzdmap[:, :, 2] > 200] = hzdmap[hzdmap[:, :, 2] > 200]
if goal is not None:
# waypoints
for i in range(wp.shape[0]):
p = mapper.hzdmap.get_pose_pix(wp[i])
cv2.circle(ovlmap, (int(p[0]), int(p[1])), 3, (255, 0, 0),
-1)
if (i > 0):
cv2.line(ovlmap, (int(p0[0]), int(p0[1])),
(int(p[0]), int(p[1])), (200, 0, 0), 3)
p0 = p
# save images
datadir = core.get_full_path('viz/static/img')
cv2.imwrite(os.path.join(datadir, '_images_visual_map.png'),
cv2.flip(cv2.flip(ovlmap, 1), 0))
cv2.imwrite(os.path.join(datadir, '_images_cost_map.png'),
cv2.flip(cv2.flip(ovlmap, 1), 0))
cv2.imwrite(os.path.join(datadir, '_images_left.png'),
cv2.resize(imL, (imL.shape[1] / 2, imL.shape[0] / 2)))
#cv2.imwrite(os.path.join(datadir, '_images_right.png'), imR)
def put_img(uri, im, timestamp):
#data = cv2.imencode('.png', im)[1].tostring().encode('base64')
data = cv2.cv.EncodeImage(
'.png', cv2.cv.fromarray(im)).tostring().encode('base64')
requests.put('http://192.168.201.10:5000/{}'.format(uri),
data={
'data': data,
'timestamp': timestamp
})
#put_img('/images/visual_map', topmap, frame)
rate_pl.sleep()
col = int(self.meter2pix(j))
cv2.line(self.grid, (col, 0), (col, self.shape[0]), (0, 255, 0), 1)
def meter2pix(self, meter):
return meter * self.dpm
def pix2meter(self, pix):
return 1.0 * pix / self.dpm
## Sample code
if __name__ == '__main__':
from aurora.loc import pose2d
# load configure from files
rover.setup(core.get_full_path('config/rover_coords.yaml'))
setup(core.get_full_path('config/map_config.yaml'))
pose = pose2d.Pose2D()
# dummy image
im = np.zeros((480, 640, 3), dtype=np.uint8)
cv2.circle(im, (300, 300), 100, (255, 120, 20), -1)
cv2.rectangle(im, (500, 210), (580, 320), (20, 22, 180), -1)
cv2.imshow('dummy', im)
cv2.waitKey(1)
# dummy motion
m = np.array([0.1, 0, 0.02])
for i in range(100):
p = pose.update(m)
def meter2pix(self, meter):
return meter * self.dpm
def pix2meter(self, pix):
return 1.0 * pix / self.dpm
## Sample code
if __name__ == '__main__':
from aurora.loc import pose2d
# load configure from files
rover.setup(core.get_full_path('config/rover_coords.yaml'))
setup(core.get_full_path('config/map_config.yaml'))
pose = pose2d.Pose2D()
# dummy image
im = np.zeros((480, 640, 3), dtype=np.uint8)
cv2.circle(im, (300, 300), 100, (255, 120, 20), -1)
cv2.rectangle(im, (500, 210), (580, 320), (20, 22, 180), -1)
cv2.imshow('dummy', im)
cv2.waitKey(1)
# dummy motion
m = np.array([0.1, 0, 0.02])
for i in range(100):
p = pose.update(m)
def worker(self):
stamp = time.time()
## get image
frame, imL, imR = camera.get_stereo_images()
if imL is None or imR is None:
logger.error("No image found")
self.rate_pl.sleep()
return
## save image
cv2.imwrite('{}/L{:06d}.jpg'.format(self.datadir, frame), imL)
cv2.imwrite('{}/R{:06d}.jpg'.format(self.datadir, frame), imR)
## rectify, grayscale
imL, imR = camera.rectify_stereo(imL, imR)
imLg = cv2.cvtColor(imL, cv2.COLOR_BGR2GRAY)
imRg = cv2.cvtColor(imR, cv2.COLOR_BGR2GRAY)
## compute odometry
pTc = vo.update_stereo(imLg, imRg)
p = self.pose.update_from_matrix(pTc)
#print 'INFO(nav): X={:.2f}, Y={:.2f}, THETA={:.1f}'.format(p[0], p[1], math.degrees(p[2]))
#self.sendq.put('xyh {:.3f} {:.3f} {:.3f}'.format(p[0], p[1], p[2]))
self.distance += np.linalg.norm(self.prev_pose[:2] - p[:2])
self.prev_pose = p
self.sendq.put('xyhd {:.3f} {:.3f} {:.3f} {:.3f}'.format(
p[0], p[1], p[2], self.distance))
# stereo
imD = dense_stereo.disparity(imLg, imRg)
imD = np.array(imD / 16., dtype=np.float)
imD[imD < 0] = 0
#print np.unique(imD.astype(np.float32))
_imXYZ = dense_stereo.reproject(imD.astype(np.float32),
rover.Q.astype(np.float32))
_imXYZ[_imXYZ[:, :, 2] < 0] = 0
_imXYZ[_imXYZ[:, :, 2] > 10] = 0
X = np.resize(_imXYZ, (_imXYZ.shape[0] * _imXYZ.shape[1], 3)).T
Y = tfm.transformp(X, rover.bTi)
imXYZ = np.resize(Y.T, _imXYZ.shape)
#print imXYZ
#print np.unique(imXYZ[:, :, 2])
#print np.amin(imXYZ[:, :, 2]), np.amax(imXYZ[:, :, 2])
rocks = 255 * np.array(imXYZ[:, :, 2] > 0.2, dtype=np.uint8)
rocks[imD == 0] = 0
# rocks
#rocks = 255 * np.array(imLg > 200, dtype=np.uint8)
kern = np.ones((15, 15), dtype=np.uint8)
rocks = cv2.dilate(cv2.erode(rocks, kern), kern)
rocks3 = np.zeros((rocks.shape[0], rocks.shape[1], 3), dtype=np.uint8)
rocks3[rocks == 0] = 1
rocks3[:, :, 2] = rocks
#rocks3[rocks == 0] = 1
# dem
imD_mask = np.zeros(imD.shape, dtype=np.uint8)
imD_mask[10:-10, 10:-10] = 1
cv2.fillPoly(imD_mask, [
np.array([[90, 470], [10, 70], [0, 70], [0, 470]], dtype=np.int32)
], 0, 8)
imD *= imD_mask
imDEM = dem.lvd(imD)
imDEM3 = np.zeros((imDEM.shape[0], imDEM.shape[1], 3))
imDEM3[:, :, 2] = (imDEM + 3) * 255 / 6
## update map
mapmask = np.zeros(imL.shape, dtype=np.uint8)
mapmask[200:400, 120:520, :] = 1
mapper.vizmap.add_image(imL * mapmask, p)
mapper.hzdmap.add_image(rocks3 * mapmask, p)
mapper.elvmap.add_topview_image(imDEM3, p)
print "==================="
logger.debug('frame {} stamp {}'.format(frame, stamp))
logger.debug('pose={}'.format(p))
logger.debug('distance={}'.format(self.distance))
print "self.next_goal", self.next_goal
print "self.goals", self.goals.queue
#print "self.wp", self.wp
## planning
if self.next_goal is None:
try:
self.next_goal = self.goals.get_nowait()
except:
self.next_goal = None
if self.next_goal is not None:
if self.get_distance(self.next_goal, p[:2]) < 0.5:
self.sendq.put('! *** Goal reached *** ')
self.sendto_obc([
'f',
])
self.wp = np.empty((0, 3))
self.next_goal = None
if self.wp.shape[0] == 0 and self.next_goal is not None:
# generate waypoints in rover relative coordinates
self.origin = p
goal_rel = np.dot(
np.linalg.inv(self.pose.matrix_from_pose(self.origin)),
self.pose.matrix_from_pose(self.next_goal))
goal_rel = (goal_rel[0, 3], goal_rel[1, 3], 0)
self.wp = local_planner.compute_waypoints(mapper.hzdmap, (0, 0, 0),
goal_rel)
if frame % 1 == 0 and self.wp.shape[0] > 0:
# compute pose of next waypoint in local frame
wp_rel = np.dot(
np.linalg.inv(self.pose.matrix_from_pose(self.origin)),
self.pose.matrix_from_pose(self.wp[0, :]))
if self.get_distance(self.wp[0, :], np.zeros(3)) < 0.5:
self.wp = np.delete(self.wp, 0, axis=0)
# generate command
if self.wp.shape[0] > 0:
next_wp_rel = self.wp[0, :].ravel()
oTp = np.dot(
np.linalg.inv(self.pose.matrix_from_pose(self.origin)),
self.pose.matrix_from_pose(p))
p_rel = self.pose.pose_from_matrix(oTp)
#print next_wp_rel
#print p_rel
cmd_arc = self.get_distance(next_wp_rel, p_rel)
cmd_theta = -math.degrees(
np.arctan2(next_wp_rel[1] - p_rel[1],
next_wp_rel[0] - p_rel[0]))
logger.debug('R={:.2f} THETA={:.2f}'.format(
cmd_arc, cmd_theta))
cmd_list = []
if abs(cmd_theta) > 165:
# back
obc.set_turn_mode(False)
cmd_list.append('d{:.2f}'.format(-cmd_arc))
elif abs(cmd_theta) > 90:
# don't handle backwards waypoint. shutdown planning
obc.set_turn_mode(False)
cmd_list.append('f')
logger.warn(
' *** AKI never goes back! (Planning canceled) ***')
self.next_goal = None
elif abs(cmd_theta) > 45:
# back step
obc.set_turn_mode(False)
obc.set_steer_angle(0)
#cmd_list.append('d{:.2f}'.format(-cmd_arc))
cmd_list.append('d{:.2f}'.format(-1))
else: #if abs(cmd_theta) <= 45:
# steering drive
obc.set_turn_mode(False)
obc.set_steer_angle(cmd_theta)
cmd_list.append('d{:.2f}'.format(cmd_arc))
'''
else:
# Inspot turn
obc.set_turn_mode(True)
cmd_list.append('r{:.2f}'.format(cmd_theta))
'''
self.sendto_obc(cmd_list)
## save images
if frame % 1 == 0:
topmap = mapper.vizmap.get_map(trajectory=True,
grid=True,
centered=True)
cv2.circle(topmap, (topmap.shape[1] / 2, topmap.shape[0] / 2), 25,
(140, 20, 130), 4)
elvmap = mapper.elvmap.get_map(trajectory=True,
grid=False,
centered=True)
#print np.unique(elvmap)
hzdmap = mapper.hzdmap.get_map(trajectory=True,
grid=True,
centered=True)
ovlmap = topmap.copy()
ovlimL = imL.copy()
ovlmap[hzdmap[:, :, 2] > 200] = hzdmap[hzdmap[:, :, 2] > 200]
# draw waypoints
if self.wp.shape[0] > 0:
p0 = mapper.vizmap.get_pose_pix(np.zeros(3))
a0 = (imL.shape[1] / 2, 1.5 * imL.shape[0])
wTo = self.pose.matrix_from_pose(self.origin)
wTr = self.pose.matrix_from_pose(self.pose.update((0, 0, 0)))
for i in range(self.wp.shape[0]):
oTwp = self.pose.matrix_from_pose(self.wp[i])
rTwp = np.dot(np.dot(np.linalg.inv(wTr), wTo), oTwp)
wp_r = self.pose.pose_from_matrix(rTwp)
# topview
p = mapper.vizmap.get_pose_pix(wp_r)
cv2.line(ovlmap, (int(p0[0]), int(p0[1])),
(int(p[0]), int(p[1])), (200, 0, 0), 3)
cv2.circle(ovlmap, (int(p[0]), int(p[1])), 5, (0, 0, 255),
-1)
p0 = p
# left
pp = np.array([[wp_r[0]], [wp_r[1]], [0]])
if wp_r[0] > 0.5:
a = tfm.projectp(pp, rover.KL, rover.iTb)
cv2.line(ovlimL, (int(a0[0]), int(a0[1])),
(int(a[0]), int(a[1])), (200, 0, 0), 6)
cv2.circle(ovlimL, (int(a[0]), int(a[1])), 8,
(0, 0, 255), -1)
a0 = a
for g in self.goals.queue:
wTg = self.pose.matrix_from_pose(g)
rTg = np.dot(np.linalg.inv(wTr), wTg)
g_rel = self.pose.pose_from_matrix(rTg)
p = mapper.vizmap.get_pose_pix(g_rel)
cv2.circle(ovlmap, (int(p[0]), int(p[1])), 5,
(200, 0, 255), -1)
datadir = core.get_full_path('viz/static/img')
cv2.imwrite(os.path.join(datadir, '_images_left.png'), ovlimL)
cv2.imwrite(os.path.join(datadir, '_images_visual_map.png'),
cv2.flip(cv2.flip(ovlmap, 1), 0))
cv2.imwrite(
os.path.join(datadir, '_images_disparity.png'),
cv2.resize(255 * plt.cm.jet(imD.astype(np.uint8)), (320, 240)))
cv2.imwrite(
os.path.join(datadir, '_images_elev_map.png'),
cv2.flip(cv2.flip(elvmap[:, :, 2].astype(np.uint8), 1), 0))
cv2.imwrite(
os.path.join(datadir, '_images_hazard_map.png'),
cv2.flip(cv2.flip(hzdmap[:, :, 2].astype(np.uint8), 1), 0))
#cv2.imwrite(os.path.join(datadir, '_images_elev_map.png'), cv2.flip(cv2.flip(255 * plt.cm.jet(elvmap[:, :, 2].astype(np.uint8)), 1), 0))
#cv2.imwrite(os.path.join(datadir, '_images_elev_map.png'),
#cv2.flip(cv2.transpose(cv2.flip(255 * plt.cm.jet(imDEM.astype(np.uint8)), 1)), 1))
if frame % 15 == 0:
self.wp = np.empty((0, 3))
self.rate_pl.sleep()
@property
def pos(self):
return self.ekf.x[:3]
@property
def cov(self):
return np.sqrt(np.diag(self.ekf.P)[:3])
## Sample code
if __name__ == '__main__':
import matplotlib.pyplot as plt
pose = Pose2DEKF()
pose.setup(core.get_full_path('config/ekf_config.yaml'))
pose.set_pose(np.array([0, 0, 0, 2, 0, 0]))
pos = np.zeros((0, 6))
cnv = np.zeros((0, 6))
z = np.array([2, 0, 0])
for i in range(50):
pose.update(z)
pose.predict()
pos = np.vstack((pos, pose.pos))
cnv = np.vstack((cnv, pose.cov))
z = np.array([2, 0, 0.1])
for i in range(50):
