def nearest():
lat = request.args.get('lat', '')
lng = request.args.get('lng', '')
routing = Routing(con)
node_id = routing.get_nearest_node(lat,lng)
return jsonify({'id': node_id})
def plot_line(topo, proto, label, color, linestyle='-', linewidth=2):
start = time.time()
routing = Routing(topo, proto, log)
paths = routing.generate_random_permutation_paths()
edge_counts = sorted(routing.edge_counts(paths).values())
x = [0]
y = [0]
for i in range(len(edge_counts)):
# format graph properly
if i != 0 and edge_counts[i] != edge_counts[i - 1]:
x.append(i)
y.append(edge_counts[i - 1])
x.append(i)
y.append(edge_counts[i])
y.append(edge_counts[-1] + 1)
x.append(len(edge_counts) - 1)
plt.plot(x,
y,
color=color,
linestyle=linestyle,
linewidth=linewidth,
label=label)
finish = time.time()
log.info('{} ran in: {}'.format(label, finish - start))
def computeroute():
latfrom = request.args.get('latfrom', '')
lngfrom = request.args.get('lngfrom', '')
latto = request.args.get('latto', '')
lngto = request.args.get('lngto', '')
routing = Routing(con)
route = routing.compute_route(latfrom, lngfrom, latto, lngto)
profile = routing.get_side_profile(latfrom, lngfrom, latto, lngto)
bottomlevelprofile_formatted = [p[0] for p in profile]
lengthprofile_formatted = [p[1] for p in profile]
surfacelevel_formatted = [p[1] for p in profile]
pipediameter_formatted = [p[3] for p in profile]
bob_end_formatted = [p[4] for p in profile]
bob_start_formatted = [p[5] for p in profile]
bbb_end_formatted = [(p[4] + float(p[3])) for p in profile]
bbb_start_formatted = [(p[5] + float(p[3])) for p in profile]
display_names_formatted = [p[6] for p in profile]
return jsonify({
'route': json.loads(route),
'surface_level': surfacelevel_formatted,
'length_profile': lengthprofile_formatted,
'bottom_level': bottomlevelprofile_formatted,
'pipe_diamter': pipediameter_formatted,
'bob_start': bob_start_formatted,
'bob_end': bob_end_formatted,
'bbb_start': bbb_start_formatted,
'bbb_end': bbb_end_formatted,
'display_names': display_names_formatted
})
def __init__(self, grouping='shuffle', parallelism=1, paraNext=1):
self.parallelism = parallelism
self.paraNext = paraNext
self.processed = parallelism*[0]
self.grouping = grouping
r = Routing(type=self.grouping)
self.routingMethod = r.getRoutingMethod()
def createIncidentToIncidentMatrices(self):
distances = {}
incidentMap = self.simulation.getActiveIncidents(self.agent)
router = Routing()
for incidentType, incidents in incidentMap.iteritems():
distances[incidentType] = {}
for incidentId, pos in incidents.iteritems():
for incident2Id, pos2 in incidents.iteritems():
if incidentId != incident2Id:
if incidentId not in distances[incidentType]:
distances[incidentType][incidentId] = {}
if incident2Id in distances[
incidentType] and incidentId in distances[
incidentType][incident2Id]:
distances[incidentType][incidentId][
incident2Id] = distances[incidentType][
incident2Id][incidentId]
else:
distance = len(
router.route(pos.lat, pos.lon, pos2.lat,
pos2.lon)['timecoords'])
distances[incidentType][incidentId][
incident2Id] = distance
distances[incidentType][incidentId][
incident2Id] = distance
return distances
def test_routing(self):
self.test_field_finalization()
r = Routing(self.field)
ret = r.route(4)
self.assertGreaterEqual(ret[0], 1)
def launch():
"""
Starts the Controller:
- topo is a string with comma-separated arguments specifying what
topology to build.
e.g.: 'jellyfish,4'
- routing is a string indicating what routing mechanism to use:
e.g.: 'ecmp8', 'kshort'
"""
# NOTE: currently only support jellyfish topology.
log.info("Launching Jellyfish controller")
# Read out configuration from file.
# NOTE: assumes jellyfish has been installed in the home directory.
config_loc = 'pox/ext/__jellyconfig'
with open(config_loc, 'r', os.O_NONBLOCK) as config_file:
log.info("inside")
n = int(config_file.readline().split('=')[1])
k = int(config_file.readline().split('=')[1])
r = int(config_file.readline().split('=')[1])
seed = int(config_file.readline().split('=')[1])
routing = config_file.readline().split('=')[1].strip()
jelly_topology = topologies["jelly"](random_seed=seed, n=n, k=k, r=r)
my_routing = Routing(jelly_topology, routing, log, seed=seed)
my_routing.generate_rtable()
core.registerNew(JellyfishController, jelly_topology, my_routing)
def test_routing(self):
self.test_field_finalization()
r = Routing(self.field)
ret = r.route(4)
self.assertGreaterEqual(ret[0], 1)
def find_total_turns(total_data, start_point, end_point, total_train):
"""
Take total data and proceeding find the smallest turn.
"""
from routing import Routing
ls_instance = setup_all_lines(total_data)
path = Routing()
path.ls_lines = ls_instance
setup_state(path.all_train_history, total_train, start_point)
return path.find_smallest_turn()
def _full_simple(self, fn, nocheck=False):
field = self.construct_field()
blocks = self.parse_blocks(os.path.join(self.test_data_dir, fn))
f = self.construct_force_algo_obj(self.field, self.blocks, self.pins)
cid = fn.split("op")[1].split(".")[0]
f.step_build(DEBUG)
f1 = f.get_debug_field()
if DEBUG:
fn = draw_field(f1, "schematic_build_{}.pdf".format(cid))
f.step_initial(DEBUG)
f2 = f.get_debug_field()
if DEBUG:
fn = draw_field(f2, "schematic_init_{}.pdf".format(cid))
f.step_main(DEBUG)
f3 = f.get_debug_field()
if DEBUG:
fn = draw_field(f3, "schematic_main_{}.pdf".format(cid))
f.step_last(DEBUG)
f4 = f.get_debug_field()
if DEBUG:
f4.trim_size()
fn = draw_field(f4, "schematic_final_{}.pdf".format(cid))
# CHECK IF OVERLAPPING BLOCKS WERE FOUND
overlap = f4.has_overlapping_blocks()
if overlap:
f4.show_blocks(sortkey=("groups", "pos"))
f4.trim_size()
draw_field(f4, "schematic_final_{}.pdf".format(cid))
else:
try:
f = f4.to_field()
f.optimize_size()
f.optimize_block_dirs()
r = Routing(f)
ret = r.route(4)
fn = draw_field(f, "schematic_real_{}.pdf".format(cid))
except RoutingException as e:
if not nocheck:
print "failed with circuit {}".format(cid)
print "continuing due to 'nocheck' == True"
else:
raise e
if not nocheck:
self.assertFalse(overlap, "Found overlapping blocks!")
def _full_simple(self, fn, nocheck=False):
field = self.construct_field()
blocks = self.parse_blocks(os.path.join(self.test_data_dir, fn))
f = self.construct_force_algo_obj(self.field, self.blocks, self.pins)
cid = fn.split("op")[1].split(".")[0]
f.step_build(DEBUG)
f1 = f.get_debug_field()
if DEBUG:
fn = draw_field(f1, "schematic_build_{}.pdf".format(cid))
f.step_initial(DEBUG)
f2 = f.get_debug_field()
if DEBUG:
fn = draw_field(f2, "schematic_init_{}.pdf".format(cid))
f.step_main(DEBUG)
f3 = f.get_debug_field()
if DEBUG:
fn = draw_field(f3, "schematic_main_{}.pdf".format(cid))
f.step_last(DEBUG)
f4 = f.get_debug_field()
if DEBUG:
f4.trim_size()
fn = draw_field(f4, "schematic_final_{}.pdf".format(cid))
# CHECK IF OVERLAPPING BLOCKS WERE FOUND
overlap = f4.has_overlapping_blocks()
if overlap:
f4.show_blocks(sortkey=("groups", "pos"))
f4.trim_size()
draw_field(f4, "schematic_final_{}.pdf".format(cid))
else:
try:
f = f4.to_field()
f.optimize_size()
f.optimize_block_dirs()
r = Routing(f)
ret = r.route(4)
fn = draw_field(f, "schematic_real_{}.pdf".format(cid))
except RoutingException as e:
if not nocheck:
print "failed with circuit {}".format(cid)
print "continuing due to 'nocheck' == True"
else:
raise e
if not nocheck:
self.assertFalse(overlap, "Found overlapping blocks!")
def __init__(self, nodes, edges, minb, maxb, tx_num, tx_min, tx_max):
assert nodes > 0 and edges > 0
assert 0 <= minb <= maxb
assert 0 < tx_min <= tx_max
self.min_balance = minb
self.max_balance = maxb
self.graph = MyGraph(nodes, edges, minb, maxb)
self.MST = nx.minimum_spanning_tree(self.graph.nx_graph,
weight='weight',
algorithm='prim')
self.tx = Transaction(nodes, tx_num, tx_min, tx_max)
self.routing = Routing(proxy='weight')
self.paths = {}
self.stats = Statistics()
self.dumper = Dump()
self.dumper.dump_files(self.graph, self.tx)
def test_full(self):
###
# slice the self.files list to reduce test time!!!
###
for fn in self.files[:7]:
print "-> full run - fn: {}".format(fn)
field = self.construct_field()
blocks = self.parse_blocks(os.path.join(self.test_data_dir, fn))
f = self.construct_force_algo_obj(self.field, self.blocks, self.pins)
cid = fn.split("op")[1].split(".")[0]
f.step_build(DEBUG)
f1 = f.get_debug_field()
fn = draw_field(f1, "schematic_build_{}.pdf".format(cid))
f.step_initial(DEBUG)
f2 = f.get_debug_field()
fn = draw_field(f2, "schematic_init_{}.pdf".format(cid))
f.step_main(DEBUG)
f3 = f.get_debug_field()
fn = draw_field(f3, "schematic_main_{}.pdf".format(cid))
f.step_last(DEBUG)
f4 = f.get_debug_field()
fn = draw_field(f4, "schematic_final_{}.pdf".format(cid))
# CHECK IF OVERLAPPING BLOCKS WERE FOUND
overlap = f4.has_overlapping_blocks()
if overlap:
print "-"*80
print "#### FAILED ..... FAILED ..... circ_id: {}".format(cid)
#f4.show_blocks(sortkey="pos")
#f4.show_blocks(sortkey="name")
f4.show_blocks(sortkey=("groups", "pos"))
else:
print "-"*80
print "#### {} ####".format(cid)
f = f4.to_field()
f.optimize_size()
f.optimize_block_dirs()
#f.expand_field(4)
r = Routing(f)
ret = r.route(4)
fn = draw_field(f, "schematic_real_{}.pdf".format(cid))
def main(argv):
input_dir = argv[1]
output_dir = argv[2]
for file_name in os.listdir(input_dir):
print("The order ", file_name, " is processing...")
input_path = os.path.join(input_dir, file_name)
with open(input_path, 'r', encoding='utf-8', errors='ignore') as f:
message_str = f.read()
order = Order(message_str)
mypack_obj = mypack.Pack(order)
hwpack_obj = Pack(message_str, mypack_obj=mypack_obj)
route = Routing(order, pack_obj=hwpack_obj)
nondominated_sol_listlist = route.population_global_search2(1400, 7)
for sol_list in nondominated_sol_listlist:
route.res["solutionArray"].append(route.sol_2_trucklist_Dict[''.join([str(x) for x in sol_list])])
route.save_sols(output_dir)
print("The order ", file_name, " is done.")
def viaroute():
"""
"""
routing = Routing(con)
coords = request.args.getlist('loc')
app.logger.debug('coordinates: %s', coords)
first_pair = []
segments = []
polyline_segments = []
for counter, coord in enumerate(coords):
if counter > 0:
previous = coords[counter-1].split(',')
current = coords[counter].split(',')
if counter % 2 == 0:
segment = routing.compute_route(previous[0], previous[1], current[0], current[1])
segcoords = json.loads(segment).get('coordinates')
for s in segcoords:
print "s:", s
polyline_segments.append((s[1], s[0]))
segments.append(json.loads(segment))
return jsonify({
"hint_data": {
"locations": [],
"checksum": 0000000001
},
"route_name": ["Purmerend Riolering"],
"via_indices": [],
"found_alternative": False,
"route_summary": {
"end_point": "O 2206",
"start_point": "",
"total_time": 0,
"total_distance": 0
},
"via_points": [],
"route_geometry": PolylineCodec().encode(polyline_segments),
"status_message": "Found route between points",
"status": 0
})
def createResponderToIncidentMatrices(self):
responders = self.simulation.getResponders(self.agent)
incidents = self.simulation.getActiveIncidents(self.agent)
distances = {}
router = Routing()
for t, respondersByType in responders.iteritems():
distances[t] = {}
for responderId, data in respondersByType.iteritems():
lat, lon = data["route"][0]
distances[t][responderId] = {}
incidentType = "fire" if t == "fireFighters" else "injury" if t == "ambulances" else "looters"
for incident, incidentPos in incidents[incidentType].iteritems(
):
distance = len(
router.route(lat, lon, incidentPos.lat,
incidentPos.lon)['timecoords'])
distances[t][responderId][incident] = distance
return distances
def __init__(self, with_reconstruction=True):
super(CapsNet, self).__init__()
self.with_reconstruction = with_reconstruction
self.conv1 = nn.Conv2d(1, 256, 9)
self.primary_caps = nn.Conv2d(256, 32 * 8, 9, stride=2)
self.digit_caps = Routing(32 * 6 * 6, 10, 8, 16, 32)
if with_reconstruction:
self.fc1 = nn.Linear(160, 512)
self.fc2 = nn.Linear(512, 1024)
self.fc3 = nn.Linear(1024, 784)
class TestWifi(threading.Thread):
def __init__(self, port, id):
threading.Thread.__init__(self)
self.id = id
self.w = WifiReal(port, id)
self.r = Routing(self.w)
self.t = Topology(self.r)
def prt(self, msg):
print("APP {}: {}".format(self.id, msg))
def run(self):
# print(self.module.get_visible_macs())
while (True):
self.prt("Starting cycle")
self.w.run()
# self.t.run()
self.prt(self.r.get_graph())
s_time = time.time()
while (time.time() - s_time < 5 + (randint(0, 20) / 5)):
self.w.listen_for_lines()
def __init__(self, port, id):
threading.Thread.__init__(self)
self.id = id
self.w = WifiReal(port, id)
self.r = Routing(self.w)
self.t = Topology(self.r)
from flask import Flask
import pymysql
pymysql.install_as_MySQLdb()
try:
from .routing import Routing
except ModuleNotFoundError:
from routing import Routing
app = Flask(__name__)
Routing(app)
if __name__ == "__main__":
app.run()
def getRandomPosition(self):
lat, lon = convert2LatLon(
self.distribution(np.random.uniform(), np.random.uniform()))
r = Routing()
return r.snapToRoad(lat, lon)
def main(version): shared_state = State() topo = Topology(shared_state, version) app = Routing(shared_state, version) app.start_event_loop()
print "# ble", index, "pin", pin, ":", selection, "(", subblock[
pin], ")"
inputs[index * self.bitgen.inputs + pin - 1] = selection
self.bitgen.gen_lb(inputs, functions, flops)
if __name__ == '__main__':
import sys
if len(sys.argv) != 6:
sys.stderr.write(
"usage: {:s} <placement.out> <routing.out> <netlist.net> <logic.blif>\n"
.format(sys.argv[0]))
sys.exit(1)
placement = Placement(sys.argv[1])
routing = Routing(sys.argv[2])
netlist = NET(sys.argv[3])
blif = BLIF(sys.argv[4])
tracks = int(sys.argv[5]) / 2
bitgen = Bitgen(cluster_size=4,
ble_inputs=6,
lb_inputs_per_side=4,
tracks_per_direction=tracks,
mux_size=5)
fpga = FPGA(placement, routing, netlist, blif, bitgen)
fpga.generate()
import time
from graph import Graph
get_ipt = True
if get_ipt:
mesh_id = 225
mesh_mac = "882931169"
# mesh_id = 202
# mesh_mac = "-346438966"
else:
mesh_id = 79
mesh_mac = "-346438577"
w = CompWifi(mesh_id, mesh_mac)
r = Routing(w)
t = Topology(r)
def recv_msg(from_mac, msg):
print("APP: from {}: {}".format(from_mac, msg))
t.register_recv_msg_handler(recv_msg)
class GetIpt(threading.Thread):
def __init__(self):
threading.Thread.__init__(self)
self.ipt = None
def run(self):
while(True):
self.ipt = input()
'/createSimulation', 'createSimulation',
'/(.+)/connectUser', 'connectUser',
'/(.+)/addAgent', 'addAgent',
'/(.+)/start', 'startSimulation',
'/(.+)/(.+)/assignToIncident', 'assignToIncident',
'/(.+)/(.+)/incidents', 'incidents',
'/(.+)/(.+)/score', 'score',
'/postScore', 'postScore',
'/scoreboard', 'scoreboard',
'/(.+)/(.+)/responders', 'responders',
'/availableSimulations', 'activeSimulations',
'/route', 'route',
'/images/(.+)', 'images',
'/(.+)', 'fileLoader')
routing = Routing()
manager = Manager()
class Application(web.application):
def run(self, url="0.0.0.0", port=8080, *middleware):
func = self.wsgifunc(*middleware)
return web.httpserver.runsimple(func, (url, port))
class disaster:
# exampleURL:
# http://localhost:1234/disaster
def GET(self):
page = web.template.frender('html/index.html')
return page()
class agentDisaster:
# exampleURL:
class Map_To_Graph:
def __init__(self):
self.brushfire_cffi = Cffi()
self.topology = Topology()
self.routing = Routing()
self.coverage = Coverage()
# Origin is the translation between the (0,0) of the robot pose and the
# (0,0) of the map
self.origin = {}
self.origin['x'] = 0
self.origin['y'] = 0
self.resolution = 0.05
self.step = 0
# Load map's translation
if rospy.has_param('origin'):
translation = rospy.get_param('origin')
self.origin['x'] = translation[0]
self.origin['y'] = translation[1]
if rospy.has_param('resolution'):
self.resolution = rospy.get_param('resolution')
# Initilize sensor's specs
self.sensor_names = []
self.sensor_direction = []
self.sensor_fov = []
self.sensor_range = []
self.sensor_shape = []
self.sensor_reliability = []
self.min_distance = 0
self.navigation_target = 0
# Read sensor's specs
self.robot_radius = rospy.get_param('robot_radius')
self.sensor_names = rospy.get_param('sensor_names')
self.sensor_number = len(self.sensor_names)
self.navigation_target = rospy.get_param('navigation_target')
self.navigation_pattern = rospy.get_param('navigation_pattern')
self.fixed_step = rospy.get_param('fixed_step')
for name in self.sensor_names:
self.sensor_direction.append(
rospy.get_param(name + '/sensor_direction'))
self.sensor_fov.append(rospy.get_param(name + '/fov'))
self.sensor_range.append(rospy.get_param(name + '/range'))
self.sensor_shape.append(rospy.get_param(name + '/shape'))
self.sensor_reliability.append(
rospy.get_param(name + '/reliability'))
self.ogm_topic = '/map'
self.ogm = 0
self.ogm_raw = 0
self.ogm_width = 0
self.ogm_height = 0
self.ogm_header = 0
# Flag to wait ogm subscriber to finish
self.ogm_compute = False
self.gvd = 0
self.brush = 0
self.nodes = []
self.door_nodes = []
self.rooms = []
self.room_doors = []
self.room_type = []
self.room_sequence = []
self.entering_doors = {}
self.exiting_doors = {}
self.wall_follow_nodes = []
self.wall_follow_sequence = []
self.zig_zag_nodes = []
self.zig_zag_sequence = []
self.simple_nodes = []
self.simple_sequence = []
# Load nodes from json file
map_name = rospy.get_param('map_name')
filename = '/home/mal/catkin_ws/src/topology_finder/data/' + map_name + '.json'
with open(filename, 'r') as read_file:
self.data = json.load(read_file)
self.nodes = self.data['nodes']
self.door_nodes = self.data['doors']
self.rooms = self.data['rooms']
self.room_doors = self.data['roomDoors']
self.room_type = self.data['roomType']
if 'room_sequence' in self.data:
self.room_sequence = self.data['room_sequence']
if 'entering_doors' in self.data:
self.entering_doors = {
int(k): v
for k, v in self.data['entering_doors'].items()
}
if 'exiting_doors' in self.data:
self.exiting_doors = {
int(k): v
for k, v in self.data['exiting_doors'].items()
}
if 'wall_follow_nodes' in self.data:
self.wall_follow_nodes = self.data['wall_follow_nodes']
if 'wall_follow_sequence' in self.data:
self.wall_follow_sequence = self.data['wall_follow_sequence']
if 'zig_zag_nodes' in self.data:
self.zig_zag_nodes = self.data['zig_zag_nodes']
if 'zig_zag_sequence' in self.data:
self.zig_zag_sequence = self.data['zig_zag_sequence']
if 'simple_nodes' in self.data:
self.simple_nodes = self.data['simple_nodes']
if 'simple_sequence' in self.data:
self.simple_sequence = self.data['simple_sequence']
self.node_publisher = rospy.Publisher('/nodes', Marker, queue_size=100)
self.candidate_door_node_pub = rospy.Publisher('/nodes/candidateDoors',
Marker,
queue_size=100)
self.door_node_pub = rospy.Publisher('/nodes/doors',
Marker,
queue_size=100)
self.room_node_pub = rospy.Publisher('/nodes/rooms',
Marker,
queue_size=100)
def server_start(self):
rospy.init_node('map_to_graph')
rospy.loginfo('map_to_graph node initialized.')
# Read ogm
self.ogm_compute = True
rospy.Subscriber(self.ogm_topic, OccupancyGrid, self.read_ogm)
while self.ogm_compute:
pass
# Calculate brushfire field
self.brush = self.brushfire_cffi.obstacleBrushfireCffi(self.ogm)
# Calculate gvd from brushfire and ogm
self.gvd = self.topology.gvd(self.brush)
time.sleep(1)
# self.print_markers(self.nodes, [1., 0., 0.], self.node_publisher)
# self.print_markers(self.door_nodes, [0., 0., 1.], self.door_node_pub)
# Find sequence of rooms if not already exists
if self.room_sequence == []:
self.find_room_sequence(True)
if self.navigation_pattern == 'simple':
self.find_all_wall_nodes()
rospy.loginfo("Visualize node sequence")
self.visualise_node_sequence(self.simple_sequence)
self.simple_sequence, self.simple_nodes = self.a_priori_coverage(
self.simple_sequence, False)
# Compute length of sequence
i = 0
for temp_nodes in self.simple_nodes:
distances = cdist(np.array(temp_nodes), np.array(temp_nodes),
'euclidean')
cost = self.routing.route_length(distances,
range(len(temp_nodes)))
string = 'Length of zig zag nodes at room ' + str(
i) + ': ' + str(cost)
print(string)
i += 1
else:
self.find_best_path_wall_nodes()
self.visualise_node_sequence(self.wall_follow_sequence)
self.wall_follow_sequence, self.wall_follow_nodes = self.a_priori_coverage(
self.wall_follow_sequence)
# Save wall nodes to json
self.data['wall_follow_nodes'] = self.wall_follow_nodes
self.data['wall_follow_sequence'] = self.wall_follow_sequence
rospy.loginfo("Visualize node sequence")
self.visualise_node_sequence(self.wall_follow_sequence)
# Compute length of sequence
i = 0
for temp_nodes in self.wall_follow_nodes:
distances = cdist(np.array(temp_nodes), np.array(temp_nodes),
'euclidean')
cost = self.routing.route_length(distances,
range(len(temp_nodes)))
string = 'Length of wall follow nodes at room ' + str(
i) + ': ' + str(cost)
print(string)
i += 1
map_name = rospy.get_param('map_name')
filename = '/home/mal/catkin_ws/src/topology_finder/data/' + map_name + '.json'
with open(filename, 'w') as outfile:
data_to_json = json.dump(self.data, outfile)
print("5 secs sleep with wall_follow_sequence")
rospy.sleep(5)
self.zig_zag_sequence, self.zig_zag_nodes = self.add_zig_zag_nodes(
self.wall_follow_sequence)
self.zig_zag_sequence, self.zig_zag_nodes = self.a_priori_coverage(
self.zig_zag_sequence, False)
# Save zig zag nodes to json
self.data['zig_zag_nodes'] = self.zig_zag_nodes
self.data['zig_zag_sequence'] = self.zig_zag_sequence
rospy.loginfo("Visualize zig zag node sequence")
self.visualise_node_sequence(self.zig_zag_sequence)
# Compute length of sequence
i = 0
for temp_nodes in self.zig_zag_nodes:
distances = cdist(np.array(temp_nodes), np.array(temp_nodes),
'euclidean')
cost = self.routing.route_length(distances,
range(len(temp_nodes)))
string = 'Length of zig zag nodes at room ' + str(
i) + ': ' + str(cost)
print(string)
i += 1
map_name = rospy.get_param('map_name')
filename = '/home/mal/catkin_ws/src/topology_finder/data/' + map_name + '.json'
with open(filename, 'w') as outfile:
data_to_json = json.dump(self.data, outfile)
return
# Method to visualize sequence of nodes splited into rooms with different color
def visualise_node_sequence(self, rooms):
id = 0
for nodes in rooms:
rgb = [0, 0, 0]
i = 0
for node in nodes:
rgb[0] = i / len(nodes)
rgb[1] = min(2 * i, len(nodes)) / len(nodes)
rgb[2] = min(3 * i, len(nodes)) / len(nodes)
self.print_markers([node['position']], rgb, self.room_node_pub,
id)
rospy.sleep(0.01)
i += 1
id += 1
print("Printed entire room")
return
# Method to visualize room nodes splited into segments with different color in each one
def visualise_node_segments(self, room, id=0):
color_num = 0
color = [1., 0., 0.]
# threshold = 256/len(splited_node_route)
for segment in room:
self.print_markers(segment,
np.array(color_num) * color, self.room_node_pub,
id)
id += 1
color_num += 30 / 256
if color_num > 1:
color = (color[1:] + color[:1])
color_num -= 1
rospy.sleep(0.1)
return id
# Do uniform sampling across whole map and gather points near obstacles
def uniform_sampling(self):
# Uniform sampling on map
nodes = []
# Sampling step is half the sensor's range
min_range = int(min(self.sensor_range) / self.resolution)
max_range = int(max(self.sensor_range) / self.resolution)
step = int(1.5 * self.robot_radius / self.resolution)
step_list = []
while step <= max_range:
temp_nodes = []
step_list.append(step)
indx = np.where((self.brush > step) & (self.brush <= 2 * step)
& (self.brush <= max_range))
indx = zip(*indx)
for x, y in indx:
if not x % step and not y % step:
temp_nodes.append((x, y))
step *= 2
nodes.append(temp_nodes)
obstacles = np.zeros((self.ogm_width, self.ogm_height))
final_nodes = []
i = len(nodes) - 1
while i >= 0:
temp_nodes = nodes[i]
# self.print_markers(temp_nodes, [1., 0., 0.], self.node_publisher)
# rospy.sleep(2)
# Check if new nodes override previous ones and delete them
for point in temp_nodes:
x, y = point
indexes = self.brushfire_cffi.circularRayCastCoverageCffi(
point, self.ogm, max_range, 360, 0, 0, True)
if not len(indexes):
continue
if i == len(nodes) - 1:
obstacles[zip(*indexes)] = 1
final_nodes.append(point)
else:
temp = obstacles[zip(*indexes)]
p = len(np.where(temp > 0)[0]) / len(indexes)
if p < 0.9:
final_nodes.append((x, y))
obstacles[zip(*indexes)] = 1
i -= 1
return final_nodes, step_list
# Find sequence of rooms
def find_room_sequence(self, save_result):
rospy.loginfo("Finding room sequence...")
self.room_sequence = []
# Create graph
graph = Graph()
for room in self.room_doors:
if len(room) > 1:
for i in range(len(room)):
for j in range(i + 1, len(room)):
node_id_1 = self.door_nodes.index(room[i])
node_id_2 = self.door_nodes.index(room[j])
# Find brushfire distance (closest to real dist) between doors
dist = self.brushfire_cffi.pointToPointBrushfireCffi(tuple(room[i]), \
tuple(room[j]), self.ogm)
if dist < 0:
rospy.loginfo("Error computing door distance!")
return
graph.add_edge(node_id_1, node_id_2, dist)
graph.add_edge(node_id_2, node_id_1, dist)
# Calculate graph's distances between all nodes
doors_length = len(self.door_nodes)
if doors_length > 2:
distances = np.zeros((doors_length, doors_length),
dtype=np.float64)
for i in range(doors_length):
for j in range(i + 1, doors_length):
_, _, _, dist = find_path(graph, i, j)
distances[i][j] = dist
distances[j][i] = distances[i][j]
# Call hill climb algorithm
max_iterations = 500 * doors_length
# door_route, cost, iter = self.routing.hillclimb(distances, max_iterations)
door_route, cost, iter = self.routing.random_restart_hillclimb(
distances, max_iterations)
print('Optimal RRHC room sequence', door_route,
self.routing.route_length(distances, door_route))
# door_route, cost, iter = self.routing.anneal(distances, max_iterations, 1.0, 0.95)
# print('Optimal ANNEAL room sequence', door_route, self.routing.route_length(distances, door_route))
#Sub-optimal / greedy sequence
greedy_route = []
greedy_route.append(door_route[0])
for i in range(1, doors_length):
#find min
previous = greedy_route[i - 1]
unvisited = []
for j in range(doors_length):
if j not in greedy_route:
unvisited.append(j)
minimum = distances[previous, unvisited[0]]
min_idx = unvisited[0]
for idx in unvisited:
if distances[previous][idx] < minimum:
minimum = distances[previous][idx]
min_idx = idx
greedy_route.append(min_idx)
greedy_cost = self.routing.route_length(distances, greedy_route)
print('Greedy room sequence', greedy_route, greedy_cost)
elif doors_length == 2:
door_route = [0, 1]
_, _, _, dist = find_path(graph, 0, 1)
print('Only 2 doors with distance ', dist)
elif self.door_nodes != []:
door_route = [0]
print('One door found!')
else:
door_route = []
self.room_sequence = [0]
print('One room only!')
# Correspond door route to room route
for door in door_route:
for i in range(len(self.room_doors)):
if self.door_nodes[door] in self.room_doors[
i] and i not in self.room_sequence:
self.room_sequence.append(i)
enter = {}
leave = {}
if doors_length >= 2:
# Compute enter & exit doors of each room according to found room_sequence
room_i = 1 # Start from 2nd room
room_idx = self.room_sequence[room_i]
entered = False
# Simulate door/room sequence on the graph
for i in range(len(door_route)):
# Find path from current to next door
door_idx = door_route[i]
next_door_idx = door_route[(i + 1) % len(door_route)]
# print('Room', room_idx)
path = find_path(graph, door_idx, next_door_idx)
# print('Path',path.nodes)
if entered and len(
path.nodes
) > 1: # if already inside room, first node of path is already used
ii = 1
else:
ii = 0
# Transverse through path and check for corresponding rooms
while ii < len(path.nodes):
node_idx = path.nodes[ii]
door = self.door_nodes[node_idx]
if door in self.room_doors[room_idx]:
# print('Door',node_idx, door,'in room', room_idx)
if not entered:
enter[room_idx] = door
entered = not entered
if len(self.room_doors[room_idx]) == 1:
ii -= 1 # To revisit this door
else:
leave[room_idx] = door
entered = not entered
room_i += 1
room_idx = self.room_sequence[room_i % len(
self.room_sequence)]
# print('Room changed', room_idx)
ii -= 1 # To revisit this door
ii += 1
elif self.door_nodes != []:
for i in range(len(self.room_sequence)):
enter[i] = self.door_nodes[0]
leave[i] = self.door_nodes[0]
self.entering_doors = enter
self.exiting_doors = leave
# # Print results per room (blue - entering door, read - exiting door, green - room nodes)
# for room_idx in self.room_sequence:
# print('Room',room_idx, 'all doors',self.room_doors[room_idx])
# self.print_markers([enter[room_idx]], [0,0,1], self.node_publisher)
# self.print_markers([leave[room_idx]], [1,0,0], self.door_node_pub)
# self.print_markers(self.rooms[room_idx], [0,1,0], self.room_node_pub)
# rospy.sleep(4)
if save_result:
# Save room sequence
self.data['room_sequence'] = self.room_sequence
self.data['entering_doors'] = self.entering_doors
self.data['exiting_doors'] = self.exiting_doors
map_name = rospy.get_param('map_name')
filename = '/home/mal/catkin_ws/src/topology_finder/data/' + map_name + '.json'
with open(filename, 'w') as outfile:
data_to_json = json.dump(self.data, outfile)
return
# Find the yaw that maximizes the covered walls of a given node
def find_best_yaw(self, node, next_node, loop_threshold, obstacle_weight,
rotation_weight):
yaw_between_nodes = math.degrees(
math.atan2(next_node[1] - node[1], next_node[0] - node[0]))
yaw = []
steps = int(max(self.sensor_range) / self.resolution)
# Find reachable obstacles
obstacles = self.brushfire_cffi.inRangeObstacleBrushfireCffi(
node, self.ogm, steps, True)
if len(obstacles) == 0:
return yaw
# In case of many obstacles more poses are needed for full coverage
found = 0
while found < loop_threshold * len(obstacles) and len(obstacles):
best_evaluation = 0
best_yaw = 0
best_found = 0
best_covered_obstacles = []
for angle in range(-180, 180, 10):
pose = [node[0], node[1], angle]
all_covered_obstacles = self.coverage.checkNearbyObstacleCover(
pose)
covered_obstacles = []
for ob in all_covered_obstacles:
if ob in obstacles:
covered_obstacles.append(ob)
if not len(covered_obstacles):
continue
# Absolute yaw of current and next node in [0,180]
next_rotation = np.abs(angle - yaw_between_nodes)
while next_rotation >= 360:
next_rotation -= 360
if next_rotation > 180:
next_rotation = 360 - next_rotation
# Evaluate candidate angle
# Use Motor Schema with covered area and rotation to next node as parameters
evaluation = obstacle_weight * len(covered_obstacles) / len(
obstacles) + rotation_weight * (180 - next_rotation) / 180
if evaluation > best_evaluation:
best_evaluation = evaluation
best_yaw = angle
best_found = len(covered_obstacles)
best_covered_obstacles = covered_obstacles
found += best_found
# Check if obstacle is coverable
if best_evaluation:
yaw.append(best_yaw)
else:
break
# Speed up process bypassing deletions if all obstacles are covered
if found >= loop_threshold * len(obstacles):
break
else:
# Discard checked obstacles
for ob in best_covered_obstacles:
if ob in obstacles:
obstacles.remove(ob)
yaw = list(set(yaw))
return yaw
# Find the yaw that maximizes the covered walls of a given node
def find_yaw(self, node, next_node, obstacle_weight, rotation_weight):
yaw_between_nodes = math.degrees(
math.atan2(next_node[1] - node[1], next_node[0] - node[0]))
yaw = []
steps = int(max(self.sensor_range) / self.resolution)
node_brush = self.brush[node]
steps = max(steps, node_brush)
# Find reachable obstacles
obstacles = self.brushfire_cffi.inRangeObstacleBrushfireCffi(
node, self.ogm, steps, True)
if len(obstacles) == 0:
return yaw
# In case of many obstacles more poses are needed for full coverage
found = 0
best_evaluation = 0
best_yaw = 0
for angle in range(-180, 180, 10):
pose = [node[0], node[1], angle]
all_covered_obstacles = self.coverage.checkNearbyObstacleCover(
pose)
covered_obstacles = 0
for ob in all_covered_obstacles:
if ob in obstacles:
covered_obstacles += 1
if not covered_obstacles:
continue
# Absolute yaw of current and next node in [0,180]
next_rotation = np.abs(angle - yaw_between_nodes)
while next_rotation >= 360:
next_rotation -= 360
if next_rotation > 180:
next_rotation = 360 - next_rotation
# Evaluate candidate angle
# Use Motor Schema with covered area and rotation to next node as parameters
evaluation = obstacle_weight * covered_obstacles / len(
obstacles) + rotation_weight * (180 - next_rotation) / 180
if evaluation > best_evaluation:
best_evaluation = evaluation
best_yaw = angle
# Check if obstacle is coverable
if best_evaluation:
yaw.append(best_yaw)
return yaw
def find_weights(self):
if self.navigation_target == 'cover_first':
angles = []
for s in range(self.sensor_number):
theta = self.sensor_direction[s]
while theta < 0:
theta += 360
while theta >= 360:
theta -= 360
if theta > 180:
theta = 360 - theta
if theta > 90:
theta = 180 - theta
angles.append((90 - theta) / 90)
obstacle_weight = 1 + np.mean(angles)
rotation_weight = 2 - np.mean(angles)
elif self.navigation_target == 'fast_first':
angles = []
for s in range(len(self.sensor_number)):
theta = self.sensor_direction[s]
fov = self.sensor_fov[s] / 2
while theta < 0:
theta += 360
while theta >= 360:
theta -= 360
if theta > 180:
theta = 360 - theta
if theta > 90:
theta = 180 - theta
theta = min(theta + fov, 90)
angles.append((90 - theta) / 90)
obstacle_weight = 1 + np.mean(angles)
rotation_weight = 2 - np.mean(angles)
else:
obstacle_weight = 1
rotation_weight = 1
return obstacle_weight, rotation_weight
# Add zig zag nodes inside already computed node sequence
def add_zig_zag_nodes(self, wall_follow_sequence):
zig_zag_nodes = []
zig_zag_sequence = []
filled_ogm = self.topology.doorClosure(self.door_nodes, self.ogm,
self.brushfire_cffi)
j = 0
for room in wall_follow_sequence:
temp_nodes = []
temp_sequence = []
room_brush = self.brushfire_cffi.pointBrushfireCffi(
self.rooms[j], filled_ogm)
for i in range(len(room)):
if not i:
temp_nodes.append(room[i]['position'])
temp_sequence.append(room[i])
continue
node = room[i]['position']
previous_node = room[i - 1]['position']
x_final, y_final = 0, 0
if previous_node[0] == node[0] and np.abs(
previous_node[1] -
node[1]) and np.abs(previous_node[1] - node[1]) <= max(
self.sensor_range) / self.resolution:
dif = np.abs(previous_node[1] - node[1])
x1 = int(node[0] - dif / 2)
x2 = int(node[0] + dif / 2)
y = int(np.min([previous_node[1], node[1]]) + dif / 2)
if self.brush[x1, y] >= self.brush[x2, y]:
x_final = x1
y_final = y
yaw = math.degrees(
math.atan2(y - node[1], x1 - node[0]))
else:
x_final = x2
y_final = y
yaw = math.degrees(
math.atan2(y - node[1], x2 - node[0]))
elif previous_node[1] == node[1] and np.abs(
previous_node[0] -
node[0]) and np.abs(previous_node[0] - node[0]) <= max(
self.sensor_range) / self.resolution:
dif = np.abs(previous_node[0] - node[0])
y1 = int(node[1] - dif / 2)
y2 = int(node[1] + dif / 2)
x = int(np.min([previous_node[0], node[0]]) + dif / 2)
if self.brush[x, y1] >= self.brush[x, y2]:
x_final = x
y_final = y1
yaw = math.degrees(
math.atan2(y1 - node[1], x - node[0]))
else:
x_final = x
y_final = y2
yaw = math.degrees(
math.atan2(y2 - node[1], x - node[0]))
if room_brush[x_final][y_final] > 1:
temp_nodes.append((x_final, y_final))
temp_sequence.append({
'position': (x_final, y_final),
'yaw': yaw
})
temp_nodes.append(room[i]['position'])
temp_sequence.append(room[i])
zig_zag_nodes.append(temp_nodes)
zig_zag_sequence.append(temp_sequence)
j += 1
return zig_zag_sequence, zig_zag_nodes
# Find nodes for wall following coverage
def find_all_wall_nodes(self):
rospy.loginfo("Finding simple strategy nodes...")
self.simple_nodes = []
self.simple_sequence = []
obstacle_weight, rotation_weight = 1, 1
# Uniform sampling on map
nodes = []
step = int(self.fixed_step / self.resolution)
indx = np.where((self.brush > step) & (self.brush <= 2 * step))
indx = zip(*indx)
for x, y in indx:
if not x % step and not y % step:
nodes.append((x, y))
self.print_markers(nodes, [1., 0., 0.], self.node_publisher)
rospy.sleep(2)
# Find rooms of nodes
# Fill door holes with "wall"
filled_ogm = self.topology.doorClosure(self.door_nodes, self.ogm,
self.brushfire_cffi)
id = 0
for i in range(len(self.rooms)):
# Brushfire inside each room and gather brushed wall_follow_nodes
found_nodes = []
brush = self.brushfire_cffi.pointBrushfireCffi(
self.rooms[i], filled_ogm)
for n in nodes:
if brush[n] > 0:
found_nodes.append(n)
found_nodes.sort() # Optimize path finding
print("Found {} nodes in room {}.".format(len(found_nodes),
i)) # # DEBUG
nodes_length = len(found_nodes)
if nodes_length:
distances = cdist(np.array(found_nodes), np.array(found_nodes),
'euclidean')
print('Distances of nodes done.') # DEBUG:
found_nodes.insert(0, self.entering_doors[i])
found_nodes.append(self.exiting_doors[i])
# Call RR hillclimb to optimize path
max_iterations = 2000 * len(found_nodes)
distances = cdist(np.array(found_nodes), np.array(found_nodes),
'euclidean')
cost = self.routing.route_length(distances,
range(len(found_nodes)))
print('Route length after adding doors', cost)
# node_route, cost, iter = self.routing.hillclimb(distances, max_iterations)
fixed_edges = True
node_route, cost, iter = self.routing.random_restart_hillclimb(
distances, max_iterations, fixed_edges)
print('Second HC route length', cost, 'made iters', iter, 'from',
max_iterations)
found_nodes_with_yaw = []
k = 1 # DEBUG:
for n in range(len(found_nodes)):
print('Closest obstacles process: {}/{}'.format(
k, nodes_length))
# Find closest obstacle to get best yaw
x, y = found_nodes[node_route[n]]
x2, y2 = found_nodes[node_route[(n + 1) % len(node_route)]]
yaw = self.find_yaw((x, y), (x2, y2), obstacle_weight,
rotation_weight)
if yaw == []:
continue
for point in yaw:
temp_dict = {'position': (x, y), 'yaw': point}
found_nodes_with_yaw.append(temp_dict)
k += 1
self.simple_nodes.append(found_nodes)
self.simple_sequence.append(found_nodes_with_yaw)
# Save wall nodes to json
self.data['simple_nodes'] = self.simple_nodes
self.data['simple_sequence'] = self.simple_sequence
map_name = rospy.get_param('map_name')
filename = '/home/mal/catkin_ws/src/topology_finder/data/' + map_name + '.json'
with open(filename, 'w') as outfile:
data_to_json = json.dump(self.data, outfile)
return
# Find nodes for wall following coverage with optimal path
def find_best_path_wall_nodes(self):
rospy.loginfo("Finding wall follow nodes...")
self.wall_follow_nodes = []
self.wall_follow_sequence = []
loop_threshold = 0.6
obstacle_weight, rotation_weight = self.find_weights()
# Uniform sampling on map
nodes, self.step = self.uniform_sampling()
# self.print_markers(nodes, [1., 0., 0.], self.node_publisher)
rospy.sleep(1)
# Find rooms of nodes
# Fill door holes with "wall"
filled_ogm = self.topology.doorClosure(self.door_nodes, self.ogm,
self.brushfire_cffi)
id = 0
for i in range(0, len(self.rooms)):
# Brushfire inside each room and gather brushed wall_follow_nodes
found_nodes = []
brush = self.brushfire_cffi.pointBrushfireCffi(
self.rooms[i], filled_ogm)
for n in nodes:
if brush[n] > 0:
found_nodes.append(n)
found_nodes.sort() # Optimize path finding
print("Found {} nodes in room {}.".format(len(found_nodes),
i)) # # DEBUG
if len(found_nodes) == 0:
print("ERROR!")
continue
nodes_length = len(found_nodes)
distances = cdist(np.array(found_nodes), np.array(found_nodes),
'euclidean')
print('Distances of nodes done.') # DEBUG:
cost = self.routing.route_length(distances, range(nodes_length))
print('Sorted route length', cost)
# Call hill climb algorithm
max_iterations = 150 * nodes_length
node_route, cost, iter = self.routing.step_hillclimb(
distances, max_iterations, self.step)
print('Route of wall follow nodes found.')
print('First step HC route length', cost)
# Reorder according to step HC results
temp_route = []
for n in node_route:
temp_route.append(found_nodes[n])
# Add entering & exiting doors and rotate already found route
# so that 2nd point is the closest one to the entering door
first_route = []
enter = self.entering_doors[i]
first_route.append(enter)
closest_idx = cdist(np.array([enter]),
np.array(temp_route)).argmin()
for ii in range(len(temp_route)):
first_route.append(temp_route[(ii + closest_idx) %
nodes_length])
first_route.append(self.exiting_doors[i])
# Do another hillclimb to optimize path
max_iterations = 2000 * len(first_route)
distances = cdist(np.array(first_route), np.array(first_route),
'euclidean')
cost = self.routing.route_length(distances,
range(len(first_route)))
print('Route length after adding doors and rotating sequence',
cost)
fixed_edges = True
node_route, cost, iter = self.routing.random_restart_hillclimb(
distances, max_iterations, fixed_edges)
print('Second HC route length', cost, 'made iters', iter, 'from',
max_iterations)
final_route = []
for n in node_route:
final_route.append(first_route[n])
found_nodes_with_yaw = []
k = 1 # DEBUG:
for n in range(len(final_route)):
# print('Closest obstacles process: {}/{}'.format(k, nodes_length))
# Find closest obstacle to get best yaw
x, y = final_route[n]
x2, y2 = final_route[(n + 1) % len(final_route)]
# Find cover_first poses
yaw = self.find_best_yaw((x, y), (x2, y2), loop_threshold,
obstacle_weight, rotation_weight)
if yaw != []:
for point in yaw:
temp_dict = {'position': (x, y), 'yaw': point}
found_nodes_with_yaw.append(temp_dict)
k += 1
self.wall_follow_nodes.append(found_nodes)
self.wall_follow_sequence.append(found_nodes_with_yaw)
# Save wall nodes to json
self.data['wall_follow_best_yaw_weigths'] = {
'loop_threshold': loop_threshold,
'obstacle_weight': obstacle_weight,
'rotation_weight': rotation_weight
}
map_name = rospy.get_param('map_name')
filename = '/home/mal/catkin_ws/src/topology_finder/data/' + map_name + '.json'
with open(filename, 'w') as outfile:
data_to_json = json.dump(self.data, outfile)
return
# Do an a priori coverage with found order of nodes to eliminate the not needed
def a_priori_coverage(self, nodes, eliminate=True):
self.coverage.initCoverage()
new_sequence = []
new_nodes = []
for room in nodes:
# i = 0
new_room = []
new_room_nodes = []
for i in range(len(room)):
node = room[i]
x, y = node['position']
yaw = node['yaw']
if not i or i == len(room) - 1 or not eliminate:
self.coverage.updateCover([x, y, yaw],
publish=False,
track_specs=False)
updated = True
else:
updated = self.coverage.checkAndUpdateCover(
self.brush, [x, y, yaw], 0.85)
if updated:
new_room.append(node)
new_room_nodes.append(node['position'])
self.coverage.coverage_pub.publish(self.coverage.coverage_ogm)
new_sequence.append(new_room)
new_nodes.append(new_room_nodes)
rospy.loginfo(
"A-priori coverage done at room {0}, room nodes size changed from {1} to {2}"
.format(i, len(room), len(new_room)))
i += 1
near_obstacles = np.where(self.brush == 2)
near_obstacles_cover = self.coverage.coverage[near_obstacles]
covered_obstacles = len(np.where(near_obstacles_cover >= 80)[0])
rospy.loginfo("Estimated coverage percentage {}".format(
covered_obstacles / len(near_obstacles_cover)))
return new_sequence, new_nodes
def read_ogm(self, data):
# OGM is a 2D array of size width x height
# The values are from 0 to 100
# 0 is an unoccupied pixel
# 100 is an occupied pixel
# 50 or -1 is the unknown
self.ogm_raw = np.array(data.data)
self.ogm_width = data.info.width
self.ogm_height = data.info.height
self.ogm_header = data.header
self.ogm = np.zeros((data.info.width, data.info.height), \
dtype = np.int)
# Reshape ogm to a 2D array
for x in range(0, data.info.width):
for y in range(0, data.info.height):
self.ogm[x][y] = data.data[x + data.info.width * y]
# Initilize coverage OGM with same size width x height
self.coverage.coverage = np.zeros((self.ogm_width, self.ogm_height))
self.coverage.coverage_ogm.info = data.info
self.coverage.coverage_ogm.data = np.zeros(self.ogm_width *
self.ogm_height)
self.coverage.ogm = self.ogm
self.coverage.ogm_raw = self.ogm_raw
self.coverage.ogm_width = self.ogm_width
self.coverage.ogm_height = self.ogm_height
self.coverage.ogm_header = self.ogm_header
self.ogm_compute = False
return
# Method to publish points to rviz
def print_markers(self, nodes, color, publisher, id=0):
points = []
for point in nodes:
p = Point()
p.x = point[0] * self.resolution + self.origin['x']
p.y = point[1] * self.resolution + self.origin['y']
p.z = 0
points.append(p)
# Create Marker for nodes
marker = Marker()
marker.header.frame_id = "/map"
marker.id = id
marker.type = marker.POINTS
marker.action = marker.ADD
marker.points = points
marker.pose.orientation.w = 1.0
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.color.a = 1.0
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
# rospy.loginfo("Printing nodes!")
publisher.publish(marker)
return
def __init__(self):
self.w = WifiSim()
self.r = Routing(self.w)
self.t = Topology(self.r)
self.t.register_recv_msg_handler(self.recv_msg)
self.mac = self.t.mac
class Simulator:
graph: MyGraph
MST: nx.Graph
tx: Transaction
routing: Routing
shortcuts: []
paths: Dict[Tuple, List]
min_balance = 0
max_balance = 0
stats: Statistics
dumper: Dump
def __init__(self, nodes, edges, minb, maxb, tx_num, tx_min, tx_max):
assert nodes > 0 and edges > 0
assert 0 <= minb <= maxb
assert 0 < tx_min <= tx_max
self.min_balance = minb
self.max_balance = maxb
self.graph = MyGraph(nodes, edges, minb, maxb)
self.MST = nx.minimum_spanning_tree(self.graph.nx_graph,
weight='weight',
algorithm='prim')
self.tx = Transaction(nodes, tx_num, tx_min, tx_max)
self.routing = Routing(proxy='weight')
self.paths = {}
self.stats = Statistics()
self.dumper = Dump()
self.dumper.dump_files(self.graph, self.tx)
def setup(self):
pass
def shortcuts(self):
pass
def verify_path(self, t: Transaction.Tx, path: List) -> bool:
for i in range(len(path) - 1):
if (path[i], path[i + 1]) in self.graph.channels_obj:
ch = self.graph.channels_obj[(path[i], path[i + 1])]
else:
ch = self.graph.channels_obj[(path[i + 1], path[i])]
if (ch.alice == path[i] and ch.alice_balance < t.money) or \
(ch.bob == path[i] and ch.bob_balance < t.money):
logging.warning("ch.alice_balance < t.money")
logging.warning("stop a unavailable tx")
return False
return True
def change_balance(self, t: Transaction.Tx, path: List):
# change balance
for i in range(len(path) - 1):
if (path[i], path[i + 1]) in self.graph.channels_obj:
ch = self.graph.channels_obj[(path[i], path[i + 1])]
else:
ch = self.graph.channels_obj[(path[i + 1], path[i])]
if ch.alice == path[i]:
ch.alice_balance -= t.money
ch.bob_balance += t.money
else:
ch.alice_balance += t.money
ch.bob_balance -= t.money
def refresh_weight(self):
for (pair, channel) in self.graph.channels_obj.items():
channel.calculate_weight()
def execute_transaction(self, t: Transaction.Tx, path) -> bool:
if path is None or path is []:
return False
if not self.verify_path(t, path):
return False
self.change_balance(t, path)
return True
def route(self, t: Transaction.Tx) -> bool:
path = self.routing.single_path(self.MST,
t.source,
t.destination,
proxy='weight')
self.stats.routing_count += 1
self.paths[(t.source, t.destination)] = path
if self.execute_transaction(t, path):
self.stats.tx_success += 1
self.stats.one_time_routing_count += 1
return True
else:
return False
def reroute(self, t: Transaction.Tx) -> bool:
path = self.routing.single_path(self.graph.nx_graph,
t.source,
t.destination,
proxy='weight')
self.stats.re_routing_count += 1
if self.execute_transaction(t, path):
self.stats.tx_success += 1
self.stats.re_routing_success += 1
logging.info("reroute success")
return True
else:
self.stats.tx_fail += 1
self.stats.re_routing_fail += 1
logging.warning("reroute failed!!!")
return False
def run(self):
# self.dumper.dump_files(self.graph, self.tx)
# logging.info(self.MST.edges.data('weight'))
# self.test()
for t in self.tx.tx_list:
self.stats.tx_count += 1
if (t.source,
t.destination) in self.paths: # if exist available path
path = self.paths[(t.source, t.destination)]
if self.execute_transaction(t, path): # valid path, no routing
self.stats.no_routing_count += 1
self.stats.tx_success += 1
else: # not valid path, reroute
if not self.route(t):
self.reroute(t)
else: # don't exist path now
if not self.route(t):
self.reroute(t)
logging.info("finish %d tx" % self.stats.tx_count)
self.stats.show_stats()
def test(self):
print(self.routing.single_path(self.MST, 1, 2))
print(self.routing.single_path(self.MST, 1, 6))
print(self.routing.single_path(self.MST, 1, 8))
self.draw_graph(save=True)
def draw_graph(self, save=False):
plt.figure(figsize=[16.4, 14.8])
plt.subplot(121)
edge_labels_g = dict([((
u,
v,
), round(d['weight'],
2)) for u, v, d in self.graph.nx_graph.edges(data=True)])
pos_g = nx.spring_layout(self.graph.nx_graph)
nx.draw(self.graph.nx_graph,
pos=pos_g,
node_size=160,
with_labels=True)
nx.draw_networkx_edge_labels(self.graph.nx_graph,
pos=pos_g,
edge_labels=edge_labels_g,
font_size=16,
alpha=0.5)
plt.subplot(122)
pos_mst = nx.spring_layout(self.MST)
edge_labels_mst = dict([((
u,
v,
), round(d['weight'], 2)) for u, v, d in self.MST.edges(data=True)])
nx.draw(self.MST,
pos=pos_mst,
node_size=160,
with_labels=True,
edge_color='r',
node_color='#ffaa00')
nx.draw_networkx_edge_labels(self.MST,
pos=pos_mst,
edge_labels=edge_labels_mst,
font_size=16,
alpha=0.5)
if save:
pic_name = 'graph_' + str(self.graph.node_num) + '_' + str(
self.graph.edge_num) + '.svg'
plt.savefig('./pic/' + pic_name, format='svg')
plt.show()
def __init__(self):
self.brushfire_cffi = Cffi()
self.topology = Topology()
self.routing = Routing()
self.coverage = Coverage()
# Origin is the translation between the (0,0) of the robot pose and the
# (0,0) of the map
self.origin = {}
self.origin['x'] = 0
self.origin['y'] = 0
self.resolution = 0.05
self.step = 0
# Load map's translation
if rospy.has_param('origin'):
translation = rospy.get_param('origin')
self.origin['x'] = translation[0]
self.origin['y'] = translation[1]
if rospy.has_param('resolution'):
self.resolution = rospy.get_param('resolution')
# Initilize sensor's specs
self.sensor_names = []
self.sensor_direction = []
self.sensor_fov = []
self.sensor_range = []
self.sensor_shape = []
self.sensor_reliability = []
self.min_distance = 0
self.navigation_target = 0
# Read sensor's specs
self.robot_radius = rospy.get_param('robot_radius')
self.sensor_names = rospy.get_param('sensor_names')
self.sensor_number = len(self.sensor_names)
self.navigation_target = rospy.get_param('navigation_target')
self.navigation_pattern = rospy.get_param('navigation_pattern')
self.fixed_step = rospy.get_param('fixed_step')
for name in self.sensor_names:
self.sensor_direction.append(
rospy.get_param(name + '/sensor_direction'))
self.sensor_fov.append(rospy.get_param(name + '/fov'))
self.sensor_range.append(rospy.get_param(name + '/range'))
self.sensor_shape.append(rospy.get_param(name + '/shape'))
self.sensor_reliability.append(
rospy.get_param(name + '/reliability'))
self.ogm_topic = '/map'
self.ogm = 0
self.ogm_raw = 0
self.ogm_width = 0
self.ogm_height = 0
self.ogm_header = 0
# Flag to wait ogm subscriber to finish
self.ogm_compute = False
self.gvd = 0
self.brush = 0
self.nodes = []
self.door_nodes = []
self.rooms = []
self.room_doors = []
self.room_type = []
self.room_sequence = []
self.entering_doors = {}
self.exiting_doors = {}
self.wall_follow_nodes = []
self.wall_follow_sequence = []
self.zig_zag_nodes = []
self.zig_zag_sequence = []
self.simple_nodes = []
self.simple_sequence = []
# Load nodes from json file
map_name = rospy.get_param('map_name')
filename = '/home/mal/catkin_ws/src/topology_finder/data/' + map_name + '.json'
with open(filename, 'r') as read_file:
self.data = json.load(read_file)
self.nodes = self.data['nodes']
self.door_nodes = self.data['doors']
self.rooms = self.data['rooms']
self.room_doors = self.data['roomDoors']
self.room_type = self.data['roomType']
if 'room_sequence' in self.data:
self.room_sequence = self.data['room_sequence']
if 'entering_doors' in self.data:
self.entering_doors = {
int(k): v
for k, v in self.data['entering_doors'].items()
}
if 'exiting_doors' in self.data:
self.exiting_doors = {
int(k): v
for k, v in self.data['exiting_doors'].items()
}
if 'wall_follow_nodes' in self.data:
self.wall_follow_nodes = self.data['wall_follow_nodes']
if 'wall_follow_sequence' in self.data:
self.wall_follow_sequence = self.data['wall_follow_sequence']
if 'zig_zag_nodes' in self.data:
self.zig_zag_nodes = self.data['zig_zag_nodes']
if 'zig_zag_sequence' in self.data:
self.zig_zag_sequence = self.data['zig_zag_sequence']
if 'simple_nodes' in self.data:
self.simple_nodes = self.data['simple_nodes']
if 'simple_sequence' in self.data:
self.simple_sequence = self.data['simple_sequence']
self.node_publisher = rospy.Publisher('/nodes', Marker, queue_size=100)
self.candidate_door_node_pub = rospy.Publisher('/nodes/candidateDoors',
Marker,
queue_size=100)
self.door_node_pub = rospy.Publisher('/nodes/doors',
Marker,
queue_size=100)
self.room_node_pub = rospy.Publisher('/nodes/rooms',
Marker,
queue_size=100)
def main(version):
shared_state = State()
topo = Topology(shared_state, version)
app = Routing(shared_state, version)
app.start_event_loop()
def run():
custom_path_width = 2 # leave the value 0 and it's calculated using the drone FOV specified
# Routing( 2D-array of global coordinates specifying the corners of the map,
# plot padding,
# user defined path width in meters - used for testing
# - leave the value 0 and it's calculated using the drone FOV specified)
sohn = Routing(odin_arr, 0.00002, custom_path_width)
sohn.get_local_coordinates()
sohn.analyze_coordinates()
# get_to_route( path limit points,
# origo global coordinates,
# relay box coordinates,
# path functions )
get_to_route(sohn.get_path_limit_points(), sohn.get_origo(),
odin_relay_box, sohn.get_path_functions())
# search_route( path width (meters) - if default pass obj.get_path_width as parameter,
# path limit points,
# origo global points,
# path functions,
# drone speed (m/s) - default value 1 if 0 is passed as a parameter )
search_route(custom_path_width, sohn.get_path_limit_points(),
sohn.get_origo(), sohn.get_path_functions(), 0)
# print(cmd_string)
cmd_string = go_home(sohn.get_path_limit_points(), odin_relay_box,
sohn.get_origo())
parser(cmd_string)
# -*- coding:utf-8 -*- """ author: chuanwu.sun created: 2017-03-10 22:25 e-mail: chuanwusun at gmail.com """ from routing import Routing r = Routing()
from flask import (Flask, request)
from flask_restful import Api, reqparse, Resource
from flask_cors import CORS
from database import init_database
from routing import Routing
from configurations import configurations
from models.user import User
app = Flask(__name__)
app.secret_key = configurations['secret_key']
CORS(app, supports_credentials=True)
db = init_database(app)
api = Api(app)
Routing.add_resources(api)
if __name__ == '__main__':
with app.app_context():
db.create_all()
app.run(debug=True)
"""
# NOTE: currently only support jellyfish topology.
log.info("Launching Jellyfish controller")
# Read out configuration from file.
# NOTE: assumes jellyfish has been installed in the home directory.
config_loc = 'pox/ext/__jellyconfig'
with open(config_loc, 'r', os.O_NONBLOCK) as config_file:
log.info("inside")
n = int(config_file.readline().split('=')[1])
k = int(config_file.readline().split('=')[1])
r = int(config_file.readline().split('=')[1])
seed = int(config_file.readline().split('=')[1])
routing = config_file.readline().split('=')[1].strip()
jelly_topology = topologies["jelly"](random_seed=seed, n=n, k=k, r=r)
my_routing = Routing(jelly_topology, routing, log, seed=seed)
my_routing.generate_rtable()
core.registerNew(JellyfishController, jelly_topology, my_routing)
# for debugging
if __name__ == '__main__':
topo = 'dummy'
routing = 'ecmp'
my_topology = build_topology(topo)
my_routing = Routing(my_topology, routing, log)
my_routing.generate_rtable()