def run_route_planner(): """Runs the whole program.""" # Initialize global settings and screen. settings = Settings() pygame.init() screen = pygame.display.set_mode( (settings.screen_width, settings.screen_height)) pygame.display.set_caption("Route Planner 1.1") # Make a next button. next_button = Button(settings, screen, "Next") # Hard coded map. world = World(settings.map_width, settings.map_height) """world.matrix = [["601000", "100100", "100010", "100001", "100000", "100000"], ["100000", "100000", "100100", "100000", "100000", "100000"], ["100000", "100000", "100000", "100000", "100000", "100000"], ["100000", "100000", "100000", "100000", "100000", "100000"], ["100000", "100000", "100000", "100000", "100000", "100000"], ["100000", "100000", "100000", "100000", "100000", "100000"]]""" world.matrix = [["601000", "500100", "310010", "610001", "600000", "610000"], ["530000", "400000", "310000", "510000", "530000", "100000"], ["100000", "100000", "100000", "530000", "620000", "100000"], ["100000", "100000", "100000", "100000", "100000", "100000"], ["100000", "100000", "100000", "100000", "100000", "100000"], ["100000", "100000", "100000", "100000", "100000", "100000"]] # Create nodes for the world matrix. nodes = Nodes(world.matrix) # Make a new robot instance. robot = Robot() # Init and subscribe to mqtt broker. mqtt = Mqtt(settings.broker, settings.get_client_id(), settings.username, settings.password, settings.topic) buttons = [] # Print out nodes to check if algoritm is correct. """ for node in nodes.nodes_dict: print(node + " : ", nodes.nodes_dict[node]) """ while True: # main loop rf.update_map(world.matrix, nodes, mqtt.msgs, robot) rf.check_events(settings, robot, buttons, next_button, mqtt) rf.update_robot_route(world.matrix, nodes.nodes_dict, robot) buttons = rf.update_screen(screen, settings, world.matrix, nodes, next_button, robot) """for node in nodes.nodes_dict: print(node + " : ", nodes.nodes_dict[node])""" # if no more tasks are left, the mission is complete. if len(robot.goals) == 0: print("Mission Complete!") mqtt.client.loop_stop() mqtt.client.disconnect() break
def setUp(self): self.Graph = nxGraph # build dict-of-dict-of-dict K3 ed1, ed2, ed3 = ({}, {}, {}) self.k3adj = { 0: { 1: ed1, 2: ed2 }, 1: { 0: ed1, 2: ed3 }, 2: { 0: ed2, 1: ed3 } } self.k3edges = [(0, 1), (0, 2), (1, 2)] self.k3nodes = [0, 1, 2] self.K3 = self.Graph() self.K3.adj = self.K3._adjacency = self.K3.edge = self.k3adj self.K3.node = self.K3._nodedata = {} self.K3.node[0] = {} self.K3.node[1] = {} self.K3.node[2] = {} self.K3.n = Nodes(self.K3._nodedata, self.K3._adjacency) self.K3.e = Edges(self.K3._nodedata, self.K3._adjacency) self.K3.a = Adjacency(self.K3._adjacency)
def __init__(self, data=None, **attr): # the init could be # graph = Graph(g) where g is a Graph # graph = Graph((v,e)) where v is Graph.v like and e is Graph.e like # graph = Graph(({},e)) for an edgelist with no specified nodes # others with classmethods like # Graph.from_adjacency_matrix(m) # Graph.from_adjacency_list(l) # # should abstract the data here self._nodedata = {} # empty node attribute dict self._adjacency = {} # empty adjacency dict # the interface is n,e,a,data self.n = Nodes(self._nodedata, self._adjacency) # rename to self.nodes self.e = Edges(self._nodedata, self._adjacency) # rename to self.edges self.a = Adjacency(self._adjacency) # rename to self.adjacency self.data = {} # dictionary for graph attributes # load with data if hasattr(data,'n') and not hasattr(data,'name'): # it is a new graph self.n.update(data.n) self.e.update(data.e) self.data.update(data.data) data = None try: nodes,edges = data # containers of edges and nodes self.n.update(nodes) self.e.update(edges) except: # old style if data is not None: convert.to_networkx_graph(data, create_using=self) # load __init__ graph attribute keywords self.data.update(attr)
def logout(): body = request.get_json() res = Nodes().remove_node(body['username'], body['password']) if type(res) is dict: return jsonify(res['message']), res['code'] else: return res["message"], res['code']
def NodeInitialization (x_rob,x_ped,weight,theta,p1_goal,p2_goal,v): gScore_ = [0,0] gScore = np.dot(gScore_,weight) initial_node = Nodes() initial_node.parent = 0 initial_node.
def test_remove_table(self): nodes = Nodes() nodes.tables.append({"BBBBBBBB": {"BBBBBBBB": 1}}) EXPECTED = [{"nodeself": {"nodeself": 0}}] ARG1 = "BBBBBBBB" nodes.remove_table(ARG1) actual = nodes.tables self.assertEqual(EXPECTED, actual)
def test_add_table_byte(self): nodes = Nodes() EXPECTED = [{"nodeself": {"nodeself": 0}}, {"BBBBBBBB": {"BBBBBBBB": 1}}] ARG1 = bytearray("\x42\x42\x42\x42\x42\x42\x42\x42\x30\x30") ARG2 = 1 nodes.add_table_byte(ARG1, ARG2) actual = nodes.tables self.assertEqual(EXPECTED, actual)
def __init__(self, graph, subnodes): # TODO Can we replace nbunch_iter with set(subnodes) & set(graph)? # We lose the Error messages... self._subnodes = set(self._nbunch_iter(graph, subnodes)) self._nodedata = SubNbrDict(self._subnodes, graph._nodedata) self._adjacency = SubAdjacency(self._subnodes, graph._adjacency) self.data = graph.data self.n = Nodes(self._nodedata, self._adjacency) self.e = Edges(self._nodedata, self._adjacency) self.a = self._adjacency
def reset(self, root_id): self.__root_id = root_id if not hasattr(self, '__nodes'): node_ids = [] self.__nodes = Nodes() else: node_ids = self.__nodes.ids() self.__nodes.reset() root = Node(id=self.__root_id, name="root") root.position.pose.orientation.w = 1.0 self.__nodes.update([root]) return node_ids
def test_get_nearest_neighbor(self): nodes = Nodes() TABLES = [{"BBBBBBBB": {"BBBBBBBB": 1, "CCCCCCCC": 4, "DDDDDDDD": 2, "EEEEEEEE": 3}}, {"CCCCCCCC": {"BBBBBBBB": 4, "CCCCCCCC": 1, "DDDDDDDD": 3, "EEEEEEEE": 2}}] TARGET = "EEEEEEEE" nodes.tables.extend(TABLES) EXPECTED = "CCCCCCCC" actual = nodes.get_nearest_neighbor(TARGET) self.assertEqual(EXPECTED, actual)
def init_nodes(locations): nodes = [] for i in range(lnt): nodes.append(Nodes(locations[i][0], locations[i][1], i, lnt)) for i in range(lnt): current_node = nodes[i] print("node", str(i), "'s distance sort started") for j in range(lnt): dst_sqr_x = (current_node.x - nodes[j].x) * (current_node.x - nodes[j].x) dst_sqr_y = (current_node.y - nodes[j].y) * (current_node.y - nodes[j].y) dst = sqrt(dst_sqr_x + dst_sqr_y) nodes[i].fill_distance(int(round(dst)), j) return nodes
def test_get_full_table(self): nodes = Nodes() TABLES = [{"BBBBBBBB": {"BBBBBBBB": 1, "CCCCCCCC": 4, "DDDDDDDD": 2, "EEEEEEEE": 3}}, {"CCCCCCCC": {"BBBBBBBB": 4, "CCCCCCCC": 1, "DDDDDDDD": 3, "EEEEEEEE": 2}}] nodes.tables.extend(TABLES) EXPECTED = {"BBBBBBBB": 1, "CCCCCCCC": 1, "DDDDDDDD": 2, "EEEEEEEE": 2} actual = nodes.get_full_table() self.assertEqual(EXPECTED, actual)
def test_get_full_table_byte(self): nodes = Nodes() TABLES = [{"BBBBBBBB": {"BBBBBBBB": 1, "CCCCCCCC": 4, "DDDDDDDD": 2, "EEEEEEEE": 3}}, {"CCCCCCCC": {"BBBBBBBB": 4, "CCCCCCCC": 1, "DDDDDDDD": 3, "EEEEEEEE": 2}}] nodes.tables.extend(TABLES) EXPECTED = b"\x42\x42\x42\x42\x42\x42\x42\x42\x30\x31" EXPECTED += b"\x43\x43\x43\x43\x43\x43\x43\x43\x30\x31" EXPECTED += b"\x44\x44\x44\x44\x44\x44\x44\x44\x30\x32" EXPECTED += b"\x45\x45\x45\x45\x45\x45\x45\x45\x30\x32" actual = nodes.get_full_table_byte() self.assertEqual(EXPECTED, actual)
def __init__(self, system_settings, websocket, snmp_websocket, **kwargs): super(SleepyMeshBase, self).__init__(**kwargs) if 'last_syncs' not in self._defaults: self._defaults.update({'last_syncs': list()}) # Internal Members # self._mesh_awake = True self._sync_type = 'timeout' self._save_in_progress = False self._sync_average = None self._delay_average = None # Instances # # TODO: Eliminate as many dependencies as possible self.system_settings = system_settings self.websocket = websocket self.snmp_websocket = snmp_websocket self.modbus_server = ModbusServer() self.snmp_server = SNMPTrapServer(self) self.update_interfaces = UpdateInterfaces(self) self.update_in_progress = self.update_interfaces.update_in_progress self.bridge = Bridge(self.system_settings) self.uploader = Uploader(self) self.nodes = Nodes(self.system_settings) self.platforms = Platforms(self.nodes) self.networks = Networks(self) self.error = BaseError(self.system_settings) if self.system_settings.modbus_enable: system_settings_dict = self.system_settings.attr_dict() # LOGGER.debug('Modbus Attribute Dictionary: ' + str(system_settings_dict)) self.modbus_server.start(system_settings_dict) if self.system_settings.snmp_enable: self.snmp_server.start() # Overload Node Error Methods (SNMP Error Methods)# NodeError.send_snmp = self.snmp_server.send_snmp NodeError.clear_snmp = self.snmp_server.clear_snmp
def start(self): logging.info('Start') self.nodes = Nodes() self.worker_q = queue.Queue() self.worker = threading.Thread(target=node_worker, args=(self, self._addr, 'slave', self.worker_q)) self.worker.daemon = True self._syncObjConf = SyncObjConf( onReady=lambda: self._onReady(), onStateChanged=lambda os, ns: self._stateChanged(os, ns)) self._syncObj = SyncObj( f'{self._addr}:{self._raft_port}', [f'{p}:{self._raft_port}' for p in self._peers], consumers=[self.logs, self.nodes], conf=self._syncObjConf)
def make_sidewalks(street_network): # Go through each street and create sidewalks on both sides of the road. sidewalks = Sidewalks() sidewalk_nodes = Nodes() sidewalk_network = OSM(sidewalk_nodes, sidewalks, street_network.bounds) for street in street_network.ways.get_list(): sidewalk_1_nodes = [] sidewalk_2_nodes = [] # Create sidewalk nodes for prev_nid, curr_nid, next_nid in window(street.nids, 3, padding=1): curr_node = street_network.nodes.get(curr_nid) prev_node = street_network.nodes.get(prev_nid) next_node = street_network.nodes.get(next_nid) n1, n2 = make_sidewalk_nodes(street, prev_node, curr_node, next_node) sidewalk_network.add_node(n1) sidewalk_network.add_node(n2) sidewalk_1_nodes.append(n1) sidewalk_2_nodes.append(n2) # Keep track of parent-child relationship between streets and sidewalks. # And set nodes' adjacency information sidewalk_1_nids = [node.id for node in sidewalk_1_nodes] sidewalk_2_nids = [node.id for node in sidewalk_2_nodes] sidewalk_1 = Sidewalk(None, sidewalk_1_nids, "footway") sidewalk_2 = Sidewalk(None, sidewalk_2_nids, "footway") sidewalk_1.set_street_id(street.id) sidewalk_2.set_street_id(street.id) street.append_sidewalk_id(sidewalk_1.id) street.append_sidewalk_id(sidewalk_2.id) # Add sidewalks to the network sidewalk_network.add_way(sidewalk_1) sidewalk_network.add_way(sidewalk_2) return sidewalk_network
from nodes import Nodes import numpy as np import time # mapper_dict = {0: 1, # 1: 2} # callibration_dict = {0: 0.05, # 1: 0.042} active_robots = ['3803', '3735'] nodes = Nodes(active_robots) nodes.callibrate() nodes.pendle_experiment() # for tag in nodes.nodes: # # tag = 1 # # print(nodes.nodes[tag].orien) # # start_pose = nodes.nodes[tag].pose # # start_time = nodes.nodes[tag].odomMsg_time # # nodes.nodes[tag].publish_greenLed(np.array([30])) # # nodes.nodes[tag].publish_motor_vlt(np.array([10, 10])) # # time.sleep(2) # # end_pose = nodes.nodes[tag].pose # # end_time = nodes.nodes[tag].odomMsg_time # # nodes.nodes[tag].publish_motor_vlt() # # nodes.nodes[tag].publish_greenLed() # # print(nodes.nodes[tag].orien) #
async def MainParser(args): output = args.output.lower() local_machine_name = args.local_machine file_format = args.format list_formats = args.list if (list_formats): print("*", "xlsx") print("*", "csv") print("*", "dict") print("*", "html") print("*", "json") print("*", "latex") print("*", "markdown") print("*", "string") else: db = await create_pool.create() networks = db.execute_command("get_networks") services = show_services.show(sub_service=False, only_name=True) nodes = { "Name" : Nodes(), "Type" : [] } relations = { "Source" : [], "Type" : [], "Target" : [] } logging.info(_("Generando '%s'..."), output) # Empezamos con la máquina local nodes["Name"].append(local_machine_name) nodes["Type"].append("Main") # Ahora con los servidores secundarios async for network in networks: (network,) = network network_name_tmp = nodes["Name"].append(network) nodes["Type"].append("Server") relations["Source"].append(network_name_tmp) relations["Type"].append("Server") relations["Target"].append(local_machine_name) networkid = await db.return_first_result("extract_networkid", network) if (networkid is None): logging.error(_("No se pudo obtener el identificador de red de '%s'"), network) return else: (networkid,) = networkid remote_services = db.execute_command("net2services", networkid) async for service in remote_services: (service,) = service # Agregamos los servicios como nodos service_name_tmp = nodes["Name"].append(service) nodes["Type"].append("Service") # Y ahora los relacionamos relations["Source"].append(service_name_tmp) relations["Type"].append("Service") relations["Target"].append(network_name_tmp) # Por último relacionamos los servicios locales for service in services: name_tmp = nodes["Name"].append(service) nodes["Type"].append("Service") relations["Source"].append(name_tmp) relations["Type"].append("Service") relations["Target"].append(local_machine_name) # Y escribimos :D try: if (file_format == "xlsx"): nodes2file.write_xlsx(output, nodes, relations) else: if (output == "stdout"): output = None if (text := nodes2file.write(output, file_format, nodes, relations)): print(text) except ImportError as err: logging.error(_("Ocurrió un error, probablemente se requiere una dependencia para pandas: %s"), err) else: logging.info(_("Hecho: %s"), output)
def NavigationActionSampling(parent, dt, traj_duration, agentID, v, sm, p1_goal, p2_goal, ang_num, sp_num, num_obs): t = np.dot(range(0, int(traj_duration / dt)), dt) ang_del = 37.5 / 360 * np.pi sp_del = 0.4 count = 0 act_num = (2 * ang_num + 1) * (2 * sp_num + 1) Actions = [] dv_ = np.dot(range(-sp_num, sp_num + 1), sp_del) ang_accel_ = np.dot(range(-ang_num, ang_num + 1), ang_del) dv_mesh, ang_mesh = np.meshgrid(dv_, ang_accel_) dv_sampled = dv_mesh.reshape(1, len(dv_) * len(ang_accel_)) ang_sampled = ang_mesh.reshape(1, len(dv_) * len(ang_accel_)) #plt.cla() #plt.axis('equal') #plt.grid(True) #plt.autoscale(False) #print 'len(dv_), len(ang_accel_) = {},{}'.format(len(dv_),len(ang_accel_)) for i in range(len(dv_) * len(ang_accel_)): action = Nodes(parent, parent.end_time, parent.end_time + traj_duration, [], [], [], parent.theta_new) dv = dv_sampled[0][i] #print 'dv = {}'.format(dv) ang_accel = ang_sampled[0][i] count = count + 1 #initialization if (agentID == 1): init_x_ped = parent.x_ped[0] init_y_ped = parent.x_ped[1] pos = [init_x_ped, init_y_ped, 0, 0] vx_ind = 2 vy_ind = 3 #vTravellerx = pos[2] #vTravellery = pos[3] vTx = p1_goal[0][0] - pos[0] vTy = p1_goal[0][1] - pos[1] vTx = parent.x_ped[2] vTy = parent.x_ped[3] else: init_x_rob = parent.x_rob[0] init_y_rob = parent.x_rob[1] pos = [init_x_rob, init_y_rob, 0, 0] vx_ind = 5 vy_ind = 6 #vTravellerx = pos[5] #vTravellery = pos[6] vTx = p2_goal[0][0] - pos[0] vTy = p2_goal[0][1] - pos[1] vTx = parent.x_rob[2] vTy = parent.x_rob[3] #print 'x_rob = {},{}'.format(parent.x_rob[2],parent.x_rob[3]) vTx_ = vTx / np.sqrt(vTx**2 + vTy**2) * (v + dv) vTy_ = vTy / np.sqrt(vTx**2 + vTy**2) * (v + dv) trajx = [] #trajx.append(pos[0]) trajy = [] #trajy.append(pos[1]) velx = np.dot(vTx_, np.cos(np.dot(t, ang_accel))) + np.dot( vTy_, np.sin(np.dot(t, ang_accel))) vely = -np.dot(vTx_, np.sin(np.dot(t, ang_accel))) + np.dot( vTy_, np.cos(np.dot(t, ang_accel))) for j in range(0, len(t)): pos[0] = pos[0] + velx[j] * dt pos[1] = pos[1] + vely[j] * dt trajx.append(pos[0]) trajy.append(pos[1]) #plt.plot(pos[0],pos[1],'ok') time = np.add(t, parent.end_time) action.trajt = time if dv < 0: action.intent = 1 else: action.intent = 0 if agentID == 1: action.human_trajx = trajx action.human_trajy = trajy action.human_velx = velx action.human_vely = vely x_ped_ = [] x_ped_.append(pos[0]) x_ped_.append(pos[1]) x_ped_.append(velx[-1]) x_ped_.append(vely[-1]) action.x_ped = x_ped_ else: action.robot_trajx = trajx action.robot_trajy = trajy action.robot_velx = velx action.robot_vely = vely action.robot_angz = [ang_accel for k in range(len(t))] x_rob_ = [] x_rob_.append(pos[0]) x_rob_.append(pos[1]) x_rob_.append(velx[-1]) x_rob_.append(vely[-1]) action.x_rob = x_rob_ Actions.append(action) Actions_ = [] for i in range(max(num_obs, 1)): Actions_.append(Actions) #print 'Actions_[0][0].rob_pos = {}' plt.pause(0.01) return Actions_
def FullKnowledgeCollaborativePlanner(x1, x2, dt, traj_duration, weight, agentID, H, p1_goal, p2_goal, sm, v, ww, costmap, costmap0s, cthr, cres, rr, r_m2o, t_m2o): path_robot = [] path_fScore = [] path_intent = [] path_found = False initial_node = Nodes() initial_node.x_rob = x2 initial_node.x_ped = x1 initial_node.gScore_ = [0, 0] #initial_node.theta = theta_hat #initial_node.theta_new = theta_hat gScore = [] gScore.append(0) gScore_ = [] fScore = [] fScore.append(np.Inf) totalNodes = [] totalNodes.append(initial_node) closeSet = [] openSet = [] openSet.append(0) cameFrom = [] cameFrom.append(-1) rob_hScore = np.Inf min_dist = sm - 0.1 min_vel = 0.3 x_rob = initial_node.x_rob x_ped = initial_node.x_ped pathx = [] pathx_ = [] pathy = [] pathy_ = [] rob_s_angz = [] current = [] #plt.cla() while len(openSet) > 0: current_fScores = [fScore[j] for j in openSet] min_fScore = min(current_fScores) current_node_id = fScore.index(min_fScore) if current_node_id > len(totalNodes): print 'cureent node id greater than total node size, error' break current = totalNodes[current_node_id] #if current.intent ==-1: # print 'current velocity = {}'.format(current.x_ped) #print 'cn_node_id = {}'.format(current_node_id) #print 'cn node x_rob = {}'.format(current.x_rob) #print 'cn_node x_ped = {}'.format(current.x_ped) #print 'cn_node trajx = {}'.format(current.robot_trajx) #print 'cn_node trajy = {}'.format(current.robot_trajy) if (current.end_time > (traj_duration * H)) or (rob_hScore < 0.2): pathx, pathy, pathx_, pathy_, rob_s_angz, path_intent = ReconstructPathFromNode( cameFrom, current_node_id, totalNodes, agentID) #print 'path vel = {}'.format(np.sqrt((pathx[0]-pathx[1])**2+(pathy[0]-pathy[1])**2)/dt) path_found = True path_fScore = min_fScore #print 'path found' #print 'pathx_ = {}'.format(pathx_) #print 'pathy_ = {}'.format(pathy_) break #print 'openSet = {}'.format(openSet) #print 'fScore = {}'.format(fScore) print 'openSet = {}'.format(openSet) print 'current_node_id = {}'.format(current_node_id) openSet.pop(openSet.index(current_node_id)) closeSet.append(current_node_id) ang_num = 2 sp_num = 1 neighbors = SideBySideActionSampling(current, dt, traj_duration, agentID, v, sm, p1_goal, p2_goal, ang_num, sp_num) ped_intent_real = 2 ped_travel_cost = traj_duration rob_travel_cost = traj_duration #print 'len neighbors = {}'.format(len(neighbors)) for i in range(len(neighbors)): cn_node = neighbors[i] #if cn_node.intent==-1: # print 'velocity = {}'.format(cn_node.x_ped) #collision1 = CollisionCheck([cn_node.human_trajx,cn_node.human_trajy],[cn_node.robot_trajx,cn_node.robot_trajy],min_dist-0.02,min_vel) #collision2 = MapCollisionCheck(cn_node) # TODO collision1 = Check(cn_node.robot_trajx, cn_node.robot_trajy, costmap, costmap0s, cthr, cres, rr) #collision1 = Check(cn_node.robot_trajx,cn_node.robot_trajy,costmap,costmap0s,cthr,cres,rr,r_m2o,t_m2o) collision2 = Check(cn_node.human_trajx, cn_node.human_trajy, costmap, costmap0s, cthr, cres, rr) if collision1 or collision2: continue cn_node_id = len(totalNodes) #cn_node.ped_intent_history.append(current.ped_intent_history) #cn_node.ped_intent_history.append(ped_intent_real) #print 'current x_ped = {}'.format(current.x_ped) #print 'len cn node x ped, x_rob = {},{}'.format(cn_node.x_ped,cn_node.x_rob) #print 'x ped = {}'.format(x_ped) human_velx = cn_node.x_ped[2] human_vely = cn_node.x_ped[3] human_vel = np.sqrt(human_velx**2 + human_vely**2) gScore_new = np.add(current.gScore_, [rob_travel_cost, ped_travel_cost]) gScore_new = np.add(gScore_new, [0, ww * ((human_vel - v)**2)]) cn_node.gScore_ = gScore_new gScore.append( np.dot(weight, gScore_new) + np.random.normal() * 0.01) #add noise to avoid floating error cameFrom.append(current_node_id) x_ped_ = cn_node.x_ped pHx = x_ped_[0] pHy = x_ped_[1] x_rob_ = cn_node.x_rob pRx = x_rob_[0] pRy = x_rob_[1] ped_del = np.add([ pHx + x_ped_[2] * traj_duration, pHy + x_ped_[3] * traj_duration ], [-p1_goal[0], -p1_goal[1]]) rob_del = np.add([ pRx + x_rob_[2] * traj_duration, pRy + x_rob_[3] * traj_duration ], [-p2_goal[0], -p2_goal[1]]) ped_hScore = (np.sqrt(np.dot(ped_del, ped_del)) + human_vel * traj_duration) / v rob_hScore = ( np.sqrt(np.dot(rob_del, rob_del)) + np.sqrt(x_rob_[2]**2 + x_rob_[3]**2) * traj_duration) / v hScore = np.dot(weight, [rob_hScore, ped_hScore]) fScore.append(gScore[-1] + hScore) openSet.append(cn_node_id) totalNodes.append(cn_node) #print 'robot_trajx = {},{}'.format(cn_node_id,cn_node.robot_trajx) #print 'node end time = {}'.format(cn_node.end_time) #print 'robot_trajx = {}{}'.format(ccn_node.robot_trajx[0]mcn_node.robot_trajx[-1]) #plt.plot(cn_node.robot_trajx,cn_node.robot_trajx,'*r') #plt.plot(cn_node.human_trajy,cn_node.human_trajy,'*g') #plt.pause(0.1) if not path_found: print 'fk path not found after openSet cleared' print 'total node length = {}'.format(len(totalNodes)) #else : #print 'gScore = {}'.format(gScore_new) return path_found, pathx, pathy, pathx_, pathy_, rob_s_angz, path_fScore, current
def makeNode(ip,id,port): n = Nodes(ip,id,port) return n
def nodes(s): return Nodes(s.__maas, uri=u'/tags/%s/' % s.name, op='nodes')
def nodes_allocated(s): return Nodes(s, op='list_allocated')
def nodes(s): return Nodes(s)
def PomdpFollower(x1, x2, dt, traj_duration, weight, agentID, H, theta_hat, view_thr, thr, p1_goal, p2_goal, sm, v, ww, wc, wd, costmap, costmap0s, cthr, cres, rr, r_m2o, t_m2o, wh): path_robot = [] path_fScore = [] path_intent = [] pathx = [] pathy = [] pathx_ = [] pathy_ = [] robot_s_angz = [] path_fScore = np.Inf current = [] path_found = False initial_node = Nodes() initial_node.x_rob = x2 initial_node.x_ped = x1 initial_node.theta = theta_hat initial_node.theta_new = theta_hat initial_node.gScore_ = [0, 0] gScore = [] gScore.append(0) gScore_ = [] gScore_.append([0, 0]) fScore = [] fScore.append(np.Inf) fScore_ = [] fScore_.append([np.Inf, np.Inf]) totalNodes = [] totalNodes.append(initial_node) closeSet = [] openSet = [] openSet.append(0) cameFrom = [] cameFrom.append(-1) rob_hScore = np.Inf cn_node = [] cn_node.append(initial_node) current = initial_node min_dist = 0.3 min_vel = 0.3 x_rob = initial_node.x_rob x_ped = initial_node.x_ped theta = initial_node.theta if agentID == 1: agentID_ = 2 elif agentID == 2: agentID_ = 1 belief_num = len(theta) r_path_found = [] r_x = [] r_y = [] r_x_ = [] r_y_ = [] rob_s_angz = [] rollout_path_found = [] rollout_x = [] rollout_x_ = [] rollout_y = [] rollout_y_ = [] robot_s_angz = [] fk_path_found = 1 for i in range(belief_num): if theta[i] > thr: r_path_found, r_x, r_y, r_x_, r_y_, rob_s_angz, path_fScore, current_ = FullKnowledgeCollaborativePlanner( x_ped, x_rob, dt, traj_duration, weight, agentID_, H, p1_goal[i], p2_goal[i], sm, v, ww, costmap, costmap0s, cthr, cres, rr, r_m2o, t_m2o) if not r_path_found: fk_path_found = 0 rollout_path_found.append(r_path_found) rollout_x.append(r_x) rollout_x_.append(r_x_) rollout_y.append(r_y) rollout_y_.append(r_y_) robot_s_angz.append(rob_s_angz) while len(openSet) > 0 and fk_path_found: current_fScores = [fScore[j] for j in openSet] min_fScore = min(current_fScores) current_node_id = fScore.index(min_fScore) if current_node_id > len(totalNodes): print 'cureent node id greater than total node size, error' break current = totalNodes[current_node_id] if (current.end_time > traj_duration * H) or (rob_hScore < 1.0): pathx, pathy, pathx_, pathy_, rob_s_angz, path_intent = ReconstructPathFromNode( cameFrom, current_node_id, totalNodes, agentID) path_found = True path_fScore = min_fScore pathx_ = r_x pathy_ = r_y #print 'rob_s_angz = {}'.format(rob_s_angz) #print 'rob_hScore = {}'.format(rob_hScore) #print 'pomdp path vel = {}'.format(np.sqrt((pathx[0]-pathx[1])**2+(pathy[0]-pathy[1])**2)/dt) break if current_node_id in openSet: openSet.pop(openSet.index(current_node_id)) else: print 'current node id {} is not in list'.format(current_node_id) print 'totalNodes length = {}'.format(len(totalNodes)) print 'closedSet = {}'.format(closeSet) print 'openSet = {}'.format(openSet) break closeSet.append(current_node_id) theta = current.theta_new num_ti = [i for i in range(len(theta)) if theta[i] > thr] ang_num = 1 sp_num = 1 H_ = max(1, H - 2) if current.end_time < (traj_duration * H_): sp_num = 1 #print 'agent id = {}'.format(agentID) neighbors = NavigationActionSampling(current, dt, traj_duration, agentID, v, sm, p1_goal, p2_goal, ang_num, sp_num, len(num_ti)) #print 'neighbor size = {}x{}'.format(len(neighbors),len(neighbors[0])) #print 'current node start time and end time = {},{}'.format(current.start_time,current.end_time) #print 'child node start time and end time = {},{}'.format(neighbors[0][0].start_time,neighbors[0][0].end_time) ped_travel_cost = traj_duration rob_travel_cost = traj_duration start_ind = int(np.floor(current.end_time / dt)) num_traj = int(np.floor(traj_duration / dt)) RolloutX = [] RolloutY = [] RolloutX_ = [] RolloutY_ = [] #print 'start ind, s+num = {},{}'.format(start_ind,start_ind+num_traj) #print 'neighbor size = {},{}'.format(len(neighbors),len(neighbors[0])) #print 'rollout length = {},{}'.format(len(rollout_x),len(rollout_x[0])) for j in num_ti: #print 'len rollout[j] = {}'.format(len(rollout_x[j])) #print 'start index = {}, end index = {}'.format(start_ind,start_ind+num_traj) RolloutX.append([ rollout_x[j][k] for k in range(start_ind, start_ind + num_traj, 1) ]) RolloutX_.append([ rollout_x_[j][k] for k in range(start_ind, start_ind + num_traj, 1) ]) RolloutY.append([ rollout_y[j][k] for k in range(start_ind, start_ind + num_traj, 1) ]) RolloutY_.append([ rollout_y_[j][k] for k in range(start_ind, start_ind + num_traj, 1) ]) Rollout = [] Rollout.append(RolloutX) Rollout.append(RolloutY) for i in range(len(neighbors[0])): #nodes with the same sampled action cn_node = [neighbors[j][i] for j in range(len(neighbors))] traj = [] vel = [] #agent velocity x = [] # agent position #print 'robot velocity = {}'.format(cn_node[0].robot_velx[-1]**2+cn_node[0].robot_vely[-1]**2) if agentID == 1: traj.append( [cn_node[0].human_trajx[j] for j in range(num_traj)]) traj.append( [cn_node[0].human_trajy[j] for j in range(num_traj)]) traj.append( [cn_node[0].human_velx[j] for j in range(num_traj)]) traj.append( [cn_node[0].human_vely[j] for j in range(num_traj)]) vel.append(cn_node[0].x_ped[2]) vel.append(cn_node[0].x_ped[3]) x = [cn_node[0].x_ped[0], cn_node[0].x_ped[1]] else: traj.append( [cn_node[0].robot_trajx[j] for j in range(num_traj)]) traj.append( [cn_node[0].robot_trajy[j] for j in range(num_traj)]) traj.append( [cn_node[0].robot_velx[j] for j in range(num_traj)]) traj.append( [cn_node[0].robot_vely[j] for j in range(num_traj)]) #print 'cn_node vel = {}'.format(cn_node[0].x_rob) vel.append(cn_node[0].x_rob[2]) vel.append(cn_node[0].x_rob[3]) x = [cn_node[0].x_rob[0], cn_node[0].x_rob[1]] #collision1 = Check(cn_node[0].robot_trajx,cn_node[0].robot_trajy,costmap,costmap0s,cthr,cres,rr,r_m2o,t_m2o) collision1 = Check(cn_node[0].robot_trajx, cn_node[0].robot_trajy, costmap, costmap0s, cthr, cres, rr) collision2 = PartnerCollisionCheck(Rollout, traj, min_dist - 0.02, min_vel) if collision1 or collision2: #print 'collision 1,2 = {}{}'.format(collision1,collision2) #print 'Rollout = {}'.format(Rollout) #rint 'traj = {}'.format(traj) continue #print 'RolloutX_ = {}'.format(RolloutX_) #print 'vel = {}'.format(vel) wdist = 0.0 x_ = [] #partner state x__ = [] #agent state appended for j in range(len(RolloutX_)): for k in range(len(RolloutX_[j])): if np.isnan(RolloutX_[j][k]) or np.isnan(RolloutY_[j][k]): print 'rollout is nan' if np.isnan(traj[0][k]) or np.isnan(traj[1][k]): print 'traj is nan' wdist = wdist + theta[num_ti[j]] * np.sqrt( (RolloutX_[j][k] - traj[0][j])**2 + (RolloutY_[j][k] - traj[1][j])**2) x_.append([RolloutX[j][-1], RolloutY[j][-1]]) x__.append(x) if agentID == 1: x_ped_ = x__ x_rob_ = x_ else: x_rob_ = x__ x_ped_ = x_ cdist = [wdist, wdist] cdist[agentID_] = 0.0 cdist = [0, 0] #TODO #chvel = [0,ww*((v-np.sqrt(cn_node[0].x_ped[2]**2+cn_node[0].x_ped[3]**2))**2)] chvel = [0, 0] gScore_new = [0, 0] #print 'gScorw = {}'.format(current.gScore_) gScore_new[ 0] = current.gScore_[0] + rob_travel_cost + chvel[0] + cdist[0] gScore_new[ 1] = current.gScore_[1] + ped_travel_cost + chvel[1] + cdist[1] gScoreNew = np.dot(gScore_new, weight) for j in num_ti: #bayesian belief update based on obs:traj_ if len(num_ti) < belief_num: # assuming only two states theta = [0 for k in range(len(theta))] theta[j] = 1.0 else: obs = x_[j] distx = [ np.sqrt((x_[k][0] - obs[0])**2 + (x_[k][1] - obs[1])**2) for k in range(len(x_)) ] distx[j] = np.Inf obs_x = [obs[0] - x[0], obs[1] - x[1]] view = np.dot(vel, obs_x) / np.sqrt(np.dot( obs_x, obs_x)) / np.sqrt(np.dot(vel, vel)) cn_node[j].view = view if view > view_thr: #belief updates mindistx = min(distx) minind = distx.index(mindistx) #update: other obs_s1 = ObsLikelihood(wd[j], mindistx) obs_s = [1.0 - obs_s1 for k in range(belief_num)] #update: self obs_s[j] = obs_s1 s = theta #?? o = np.dot(obs_s, s) s_new = np.multiply(obs_s, s) theta = [k / o for k in s_new] cn_ind = num_ti.index(j) cn_node[cn_ind].theta_new = theta cn_node[cn_ind].gScore_ = gScore_new #cost_to_go using the updated theta #print 'p1_goal and ped_pos = {},{}'.format(p1_goal,x_ped_) #print 'p2_goal and rob_pos = {},{}'.format(p2_goal,x_rob_) ped_dtgo = [ np.sqrt((p1_goal[num_ti[k]][0] - x_ped_[k][0])**2 + (p1_goal[num_ti[k]][1] - x_ped_[k][1])**2) * theta[num_ti[k]] / v for k in range(len(x_ped_)) ] ped_hScore = np.sum(ped_dtgo) rob_dtgo = [ np.sqrt((p2_goal[num_ti[k]][0] - x_rob_[k][0])**2 + (p2_goal[num_ti[k]][1] - x_rob_[k][1])**2) * theta[num_ti[k]] / v for k in range(len(x_rob_)) ] rob_hScore = np.sum(rob_dtgo) hScore = np.dot(weight, [rob_hScore, ped_hScore]) gScore.append(gScoreNew) gScore_.append(gScore_new) fScore_.append( np.add(gScore_new, [ wh * weight[0] * rob_hScore, wh * weight[1] * ped_hScore ])) fScore.append(gScoreNew + wh * hScore + np.random.normal() * 0.01) #cost-to-go cameFrom.append(current_node_id) openSet.append(len(totalNodes)) totalNodes.append(cn_node[cn_ind]) if (len(totalNodes) - len(openSet)) > 400: print 'gScore_ = {}'.format(gScore_[-1]) print 'gScoreNew = {}'.format(gScoreNew) print 'fScore = {}'.format(fScore[-1]) #print 'too many nodes' break if not path_found: print 'pomdp path not found after openSet cleared' print 'total node length = {}'.format(len(totalNodes)) print 'cn_node end time = {}'.format(cn_node[0].end_time) print 'current node time = {}'.format(current.end_time) #else: # print 'total node length = {}'.format(len(totalNodes)) return path_found, pathx, pathy, pathx_, pathy_, robot_s_angz, path_fScore, current
def SideBySideActionSampling(parent, dt, traj_duration, agentID, v, sm, p1_goal, p2_goal, ang_num, sp_num): t = np.dot(range(0, int(np.ceil((traj_duration / dt)))), dt) ang_del = 37.5 / 360 * np.pi sp_del = 0.4 count = 0 if agentID == 1: #human sp_num = 0 else: sp_num = 1 act_num = (2 * ang_num + 1) * (2 * sp_num + 1) Actions = [] #for i in range(act_num): # Actions.append(Nodes(parent,parent.end_time,parent.end_time+traj_duration,[],[],[],[],parent.theta_new)) dv_ = np.dot(range(-sp_num, sp_num + 1), sp_del) ang_accel_ = np.dot(range(-ang_num, ang_num + 1), ang_del) dv_mesh, ang_mesh = np.meshgrid(dv_, ang_accel_) dv_sampled = dv_mesh.reshape(1, len(dv_) * len(ang_accel_)) ang_sampled = ang_mesh.reshape(1, len(dv_) * len(ang_accel_)) init_x_ped = parent.x_ped[0] init_y_ped = parent.x_ped[1] init_x_rob = parent.x_rob[0] init_y_rob = parent.x_rob[1] for i in range(act_num): action = Nodes(parent, parent.end_time, parent.end_time + traj_duration, [], [], [], [], parent.theta_new) dv = dv_sampled[0][i] ang_accel = ang_sampled[0][i] count = count + 1 #initialization if (agentID == 1): pos = [init_x_ped, init_y_ped] pos_ = [init_x_rob, init_y_rob] vx_ind = 2 vy_ind = 3 #vTravellerx = pos[2] #vTravellery = pos[3] #vTx = p1_goal[0]-pos[0] #vTy = p1_goal[1]-pos[1] vTx = parent.x_ped[2] vTy = parent.x_ped[3] else: pos = [init_x_rob, init_y_rob] pos_ = [init_x_ped, init_y_ped] vx_ind = 5 vy_ind = 6 #vTravellerx = pos[2] #vTravellery = pos[3] #vTx = p2_goal[0]-pos[0] #vTy = p2_goal[1]-pos[1] vTx = parent.x_rob[2] vTy = parent.x_rob[3] #print 'x_rob full = {},{}'.format(parent.x_rob[2],parent.x_rob[3]) #v = np.sqrt(parent.x_ped[2]**2+parent.x_ped[3]**2) vTx_ = vTx / np.sqrt(vTx**2 + vTy**2) * (v + dv) vTy_ = vTy / np.sqrt(vTx**2 + vTy**2) * (v + dv) trajx = [] #trajx.append(pos[0]) trajy = [] #trajy.append(pos[1]) trajx_ = [] #trajx_.append(pos_[0]) trajy_ = [] #trajy_.append(pos_[1]) #print 'vTx_ and t and ang_accel = {}{}{}'.format(vTx_,t,ang_accel) velx = np.dot(vTx_, np.cos(np.dot(t, ang_accel))) + np.dot( vTy_, np.sin(np.dot(t, ang_accel))) vely = -np.dot(vTx_, np.sin(np.dot(t, ang_accel))) + np.dot( vTy_, np.cos(np.dot(t, ang_accel))) for j in range(0, len(t)): pos[0] = pos[0] + velx[j] * dt pos[1] = pos[1] + vely[j] * dt s_ = np.cross([velx[j], vely[j]], np.add([pos_[0], pos_[1]], [-pos[0], -pos[1]])) the_ = np.sign(s_) * np.pi / 2 pos_[0] = pos[0] + np.dot([np.cos(the_), -np.sin(the_)], [velx[j], vely[j]]) * sm pos_[1] = pos[1] + np.dot( [np.sin(the_), np.cos(the_)], [velx[j], vely[j]]) * sm trajx.append(pos[0]) trajy.append(pos[1]) trajx_.append(pos_[0]) trajy_.append(pos_[1]) #plt.cla() #plt.axis('equal') x_min = -10 x_max = 10 y_min = 5 y_max = 20 #plt.xlim((x_min, x_max)) #plt.ylim((y_min, y_max)) #plt.grid(True) #plt.autoscale(False) #plt.plot(self.ped_pos[0],self.ped_pos[1],'ok') #plt.plot(self.rob_pos[0],self.rob_pos[1],'om') #if len(self.Xtraj)>0 : # plt.plot(self.Xtraj,self.Ytraj,'r') # plt.plot(xtraj,ytraj,'g') # plt.pause(0.1) time = np.add(t, parent.end_time + dt) action.trajt = time if dv < 0: action.intent = 1 else: action.intent = 0 if agentID == 1: action.human_trajx = trajx action.human_trajy = trajy action.human_velx = velx action.human_vely = vely action.robot_trajx = trajx_ action.robot_trajy = trajy_ action.robot_velx = velx action.robot_vely = vely x_ped_ = [] x_ped_.append(pos[0]) x_ped_.append(pos[1]) x_ped_.append(velx[-1]) x_ped_.append(vely[-1]) x_rob_ = [] x_rob_.append(pos_[0]) x_rob_.append(pos_[1]) x_rob_.append(velx[-1]) x_rob_.append(vely[-1]) action.x_rob = x_rob_ action.x_ped = x_ped_ action.robot_angz = [ang_accel for k in range(len(t))] else: action.robot_trajx = trajx action.robot_trajy = trajy action.robot_velx = velx action.robot_vely = vely action.human_trajx = trajx_ action.human_trajy = trajy_ action.human_velx = velx action.human_vely = vely x_ped_ = [] x_ped_.append(pos_[0]) x_ped_.append(pos_[1]) x_ped_.append(velx[-1]) x_ped_.append(vely[-1]) x_rob_ = [] x_rob_.append(pos[0]) x_rob_.append(pos[1]) x_rob_.append(velx[-1]) x_rob_.append(vely[-1]) action.x_rob = x_rob_ action.x_ped = x_ped_ action.robot_angz = [ang_accel for k in range(len(t))] Actions.append(action) #rotating actions ang_del = [np.pi / 1.3, -np.pi / 1.3] for ang in ang_del: action = Nodes(parent, parent.end_time, parent.end_time + traj_duration, [], [], [], [], parent.theta_new) if (agentID == 1): pos = [init_x_ped, init_y_ped] pos_ = [init_x_rob, init_y_rob] vTx = parent.x_ped[2] vTy = parent.x_ped[3] else: pos = [init_x_rob, init_y_rob] pos_ = [init_x_ped, init_y_ped] vTx = parent.x_rob[2] vTy = parent.x_rob[3] vTx_ = (vTx * np.cos(ang) - vTy * np.sin(ang)) / np.sqrt(vTx**2 + vTy**2) * 0.02 vTy_ = (vTx * np.sin(ang) + vTy * np.cos(ang)) / np.sqrt(vTx**2 + vTy**2) * 0.02 velx = np.dot(t, vTx_) vely = np.dot(t, vTy_) trajx = [vTx_ * dt * i for i in range(0, len(t))] trajy = [vTy_ * dt * i for i in range(0, len(t))] s_ = np.cross([vTx_, vTy_], np.add([pos_[0], pos_[1]], [-pos[0], -pos[1]])) the_ = np.sign(s_) * np.pi / 2 trayx_ = np.add( trajx, np.dot([np.cos(the_), -np.sin(the_)], [vTx_, vTy_]) * sm) trayy_ = np.add( trajy, np.dot([np.sin(the_), np.cos(the_)], [vTx_, vTy_]) * sm) time = np.add(t, parent.end_time + dt) action.trajt = time action.intent = -1 if agentID == 1: action.human_trajx = trajx action.human_trajy = trajy action.human_velx = velx action.human_vely = vely action.robot_trajx = trajx_ action.robot_trajy = trajy_ action.robot_velx = velx action.robot_vely = vely x_ped_ = [] x_ped_.append(trajx[-1]) x_ped_.append(trajy[-1]) x_ped_.append(velx[-1]) x_ped_.append(vely[-1]) x_rob_ = [] x_rob_.append(trajx_[0]) x_rob_.append(trajy_[1]) x_rob_.append(velx[-1]) x_rob_.append(vely[-1]) action.x_rob = x_rob_ action.x_ped = x_ped_ action.robot_angz = [ang for k in range(len(t))] else: action.robot_trajx = trajx action.robot_trajy = trajy action.robot_velx = velx action.robot_vely = vely action.human_trajx = trajx_ action.human_trajy = trajy_ action.human_velx = velx action.human_vely = vely x_ped_ = [] x_ped_.append(trajx_[0]) x_ped_.append(trajy_[1]) x_ped_.append(velx[-1]) x_ped_.append(vely[-1]) x_rob_ = [] x_rob_.append(trajx[0]) x_rob_.append(trajy[1]) x_rob_.append(velx[-1]) x_rob_.append(vely[-1]) action.x_rob = x_rob_ action.x_ped = x_ped_ action.robot_angz = [ang for k in range(len(t))] #print 'x_ped = {}'.format(action.x_ped) Actions.append(action) return Actions
def __init__(self, nodes_file): self.nodes = Nodes(nodes_file=nodes_file) self.multi_client = MultiClient(self.nodes)
from flask import Flask, jsonify, request from blockchain import Blockchain from consensus import Consensus from hash import Hash from nodes import Nodes from proof import Proof # Initialise a Node and generate a globally unique address app = Flask(__name__) node_id = str(uuid4()).replace('-', '') blockchain = Blockchain() proof = Proof() nodes = Nodes() consensus = Consensus() @app.route('/mine', methods=['GET']) def mine(): ''' Mine a new block. ''' # Get the previous block and proof and find the new proof. previous_block = blockchain.last_block previous_proof = previous_block["proof"] new_proof = proof.proof_of_work(previous_proof) # Because the user is mining a block, we want to reward them with a block,
def __init__(self, time_=0, nodes_conf_=[]): Simulator.__init__(self, time_) self.nodes_conf = [Nodes(ind, x) for ind, x in enumerate(nodes_conf_)]