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 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]])
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)
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):