def record(length, download, upload): #create csv file with headers time and speed with open(f'{datetime.date.today()}_speed.csv', mode='w', newline='', encoding='utf-8') as speedcsv: csv_writer = csv.DictWriter( speedcsv, fieldnames=['time', 'downspeed', 'upspeed']) csv_writer.writeheader() start_day = datetime.datetime.today() start_time = datetime.datetime.now() completed_time = start_day + datetime.timedelta(minutes=length) print( f"it is now {start_time} and i will stop recording at {completed_time}" ) while True: #writes into the CSV file until the time is completed changes if datetime.datetime.now() < completed_time: time = datetime.datetime.now().strftime("%H:%M:%S") downspeed = round((round(s.download()) / 1048576), 2) upspeed = round((round(s.upload()) / 1048576), 2) csv_writer.writerow({ 'time': time, 'downspeed': downspeed, "upspeed": upspeed }) print( f"time: {time}, downspeed: {downspeed} Mb/s, upspeed: {upspeed} Mb/s" ) # t.sleep(60) else: #when time is over stop writing, generate a graph, upload that graph to google drive and start again speedcsv.close() print("\nnow generating graph...") make_graph(str(start_day)[:10], download, upload) try: upload_to_drive(f"{start_day}" + "_graph.jpg") print("now uploaded to drive") except: print("API keys not correct - now saving to local device") pass print("now you can check out your graph :)") break
args = parser.parse_args() # First setup the environment map_data = np.loadtxt(args.map).astype(np.int) planning_env = MapEnvironment(map_data) <<<<<<< HEAD end = time.time() ======= >>>>>>> a7e57bfd30cce53a52a585e8e419a8029de5343d # Make a graph G = graph_maker.make_graph(planning_env, sampler=Sampler(planning_env), num_vertices=args.num_vertices, connection_radius=args.connection_radius, lazy=args.lazy) # Add start and goal nodes print(args.start, args.goal) G, start_id = graph_maker.add_node(G, args.start, env=planning_env, connection_radius=args.connection_radius) G, goal_id = graph_maker.add_node(G, args.goal, env=planning_env, connection_radius=args.connection_radius) <<<<<<< HEAD print('graph making time: ', time.time() - end) # Uncomment this to visualize the graph # planning_env.visualize_graph(G, tuple(args.start), tuple(args.goal)) =======
def __init__(self, heuristic_func, weight_func, num_vertices, connection_radius, graph_file='ros_graph.pkl', do_shortcut=False, num_goals=1, curvature=0.04): """ @param heuristic_func: Heuristic function to be used in lazy_astar @param weight_func: Weight function to be used in lazy_astar @param num_vertices: Number of vertices in the graph @param connection_radius: Radius for connecting vertices @param graph_file: File to load. If provided and the file does not exist, then the graph is constructed and saved in this filename @param do_shortcut: If True, shortcut the path @param num_goals: If > 1, takes multiple goals """ rospy.init_node('planner', anonymous=True) # load map self.map = self.get_map() self.map_x = self.map.info.origin.position.x self.map_y = self.map.info.origin.position.y self.map_angle = util.rosquaternion_to_angle( self.map.info.origin.orientation) self.map_c = np.cos(self.map_angle) self.map_s = np.sin(self.map_angle) self.map_data = self.load_permissible_region(self.map) rospy.Subscriber('/move_base_simple/goal', PoseStamped, self.get_goal) rospy.Subscriber(rospy.get_param("~pose_topic", "/sim_car_pose/pose"), PoseStamped, self.get_current_pose) self.multi_goals = num_goals > 1 self.num_goals = num_goals if self.multi_goals: print("Accept multiple goals") self.goals = [] self.route_sent = False self.do_shortcut = do_shortcut # Setup planning env self.planning_env = DubinsMapEnvironment(self.map_data.transpose(), curvature=curvature) if os.path.exists(graph_file): print("Opening {}".format(graph_file)) self.G = graph_maker.load_graph(graph_file) else: print("Generating graph, wait for completion") self.G = graph_maker.make_graph( self.planning_env, sampler=DubinsSampler(self.planning_env), num_vertices=num_vertices, connection_radius=connection_radius, saveto=graph_file, lazy=True) print("visualize graph") self.planning_env.visualize_graph(self.G, saveto="graph.png") self.num_vertices = num_vertices self.connection_radius = connection_radius self.heuristic_func = lambda n1, n2: heuristic_func( n1, n2, self.planning_env, self.G) self.weight_func = lambda n1, n2: weight_func(n1, n2, self. planning_env, self.G) print("Ready to take goals") rospy.spin()
parser.add_argument('--connection-radius', type=float, default=5.0) parser.add_argument('--lazy', action='store_true') args = parser.parse_args() times = np.zeros([1,10]) lengths = np.zeros([1,10]) # First setup the environment map_data = np.loadtxt(args.map).astype(np.int) planning_env = MapEnvironment(map_data) for i in range(9): # Make a graph G = graph_maker.make_graph(planning_env, sampler=Sampler(planning_env), num_vertices=args.num_vertices, connection_radius=args.connection_radius, directed = False, lazy=args.lazy, saveto = 'graph(%d).pkl' % i ) print("made graph") # Add start and goal nodes G, start_id = graph_maker.add_node(G, args.start, env=planning_env, connection_radius=args.connection_radius, lazy = False, start_from_config=True) G, goal_id = graph_maker.add_node(G, args.goal, env=planning_env, connection_radius=args.connection_radius, lazy = False, start_from_config=False) # Uncomment this to visualize the graph
parser.add_argument('--num-vertices', type=int, required=True) parser.add_argument('--connection-radius', type=float, default=15.0) parser.add_argument('--lazy', action='store_true') args = parser.parse_args() args.start[2] = math.radians(args.start[2]) args.goal[2] = math.radians(args.goal[2]) map_data = np.loadtxt(args.map).astype(np.int) # First setup the environment and the robot. planning_env = DubinsMapEnvironment(map_data, args.curvature) G = graph_maker.make_graph(planning_env, sampler=DubinsSampler(planning_env), num_vertices=args.num_vertices, connection_radius=args.connection_radius, lazy=args.lazy, directed=False) G, start_id = graph_maker.add_node( G, args.start, env=planning_env, connection_radius=args.connection_radius, start_from_config=True) G, goal_id = graph_maker.add_node(G, args.goal, env=planning_env, connection_radius=args.connection_radius, start_from_config=False)
parser.add_argument('--lazy', action='store_true') args = parser.parse_args() args.start[2] = math.radians(args.start[2]) args.goal[2] = math.radians(args.goal[2]) times = np.zeros([1,10]) lengths = np.zeros([1,10]) map_data = np.loadtxt(args.map).astype(np.int) # First setup the environment and the robot. planning_env = DubinsMapEnvironment(map_data, args.curvature) for i in range(9): G = graph_maker.make_graph(planning_env, sampler=DubinsSampler(planning_env), num_vertices=args.num_vertices, connection_radius=args.connection_radius, saveto = 'dubinsgraph(%d).pkl' % i) G, start_id = graph_maker.add_node(G, args.start, env=planning_env, connection_radius=args.connection_radius, lazy = True, start_from_config=True) G, goal_id = graph_maker.add_node(G, args.goal, env=planning_env, connection_radius=args.connection_radius, lazy = True, start_from_config=False) # Uncomment this to visualize the graph #planning_env.visualize_graph(G) try: heuristic = lambda n1, n2: planning_env.compute_heuristic( G.nodes[n1]['config'], G.nodes[n2]['config'])
def main(): gm.make_graph(3, 101.7321333) overlay_graph(path_of_fish, path_of_graph)