class BRAIN(MP): def init(self): self.map = MAP(self.md) def explore(self): # find most unexplored area, and drive there pass def run_impl(self): # first, check bumper if "Bumper" in self.md: print "===========================================================" print "============ BUMPER ============================" print "===========================================================" print "===========================================================" # hardcoded evade behavior self.md["Move"] = [0.2, "backward"] time.sleep(2) self.md["Move"] = [0.2, "left"] time.sleep(1) del self.md["Bumper"] #clean up from this round del self.md["lidar_points"] return # next, if we have odometry, save and react to laser input if "MCS" in self.md: self.md["WCS"] = self.md["MCS"] if "lidar_points" in self.md: # build map using laser points self.map.integrate(self.md["WCS"], self.md["lidar_points"]) #self.map.visualize(((getMs()-self.md["starttime"]) / 1000.),save=True) # save a snapshot of relevant data to files ''' tsnow = getMs() np.save("/tmp/%d_LIDAR"%tsnow, self.md["lidar_points"]) coord = np.array([self.md["WCS"].x, self.md["WCS"].y, self.md["WCS"].a]) np.save("/tmp/%d_WCS"%tsnow,coord) np.save("/tmp/%d_MAPPOINTS"%tsnow, self.map.mappoints) np.save("/tmp/%d_TILES"%tsnow, self.map.tiles) ''' free = True for i in range(10, -10, -1): m = self.md["lidar"][i] if np.isnan(m): print ".", elif m > 100 and m < 500: print "X", free = False else: print "O", print " ", print if not free: self.md["Move"] = [0.1, "left"] # was 0-100, now 0-255 else: self.md["Move"] = [0.1, "forward"] del self.md["lidar_points"] ''' # try to drive to the least known map cell target_coords = np.unravel_index(np.argmin(self.map.tiles), self.map.tiles.shape) print "Minimal knowledge at tile: " tmp = self.map.tile2coordm(target_coords[1], target_coords[0]) self.md["target"] = Coordinate(tmp[0], tmp[1], 0) self.map.wcs2rcs(self.md["target"]) # first, rotate until we are facing it # transfer map tile (WCS) to RCS to calculate relative angle ''' #time.sleep(0.1) '''