def __init__(self):
server_ip = rospy.get_param('~overpass_server_ip')
server_port = rospy.get_param('~overpass_server_port')
ref_lat = rospy.get_param('~ref_latitude')
ref_lon = rospy.get_param('~ref_longitude')
map_dimension = rospy.get_param('~map_dimension')
global_origin = [ref_lat, ref_lon]
rospy.loginfo("Server " + server_ip + ":" + str(server_port))
rospy.loginfo("Global origin: " + str(global_origin))
rospy.loginfo("Starting servers...")
self.osm_bridge = OSMBridge(server_ip=server_ip,
server_port=server_port,
global_origin=global_origin,
coordinate_system="cartesian",
debug=False)
self.osm_adapter = OSMAdapter(server_ip=server_ip,
server_port=server_port,
global_origin=global_origin,
coordinate_system="cartesian",
debug=False)
self.semantic_features_finder = SemanticFeaturesFinder(self.osm_bridge)
if map_dimension == '3D':
self.get_map = Get3DMap(self.osm_bridge,
self.semantic_features_finder)
elif map_dimension == '2D':
self.get_map = Get2DMap(self.osm_bridge, self.osm_adapter,
self.semantic_features_finder)
else:
rospy.logerr("Please specify correct map dimension")
self.get_geometric_map_server = rospy.Service('/get_geometric_map',
GetGeometricMap,
self._get_geometric_map)
rospy.loginfo(
"Get geometric map server started. Listening for queries...")
self.get_semantic_map_server = rospy.Service('/get_semantic_map',
GetSemanticMap,
self._get_semantic_map)
rospy.loginfo(
"Get semantic map server started. Listening for queries...")
self.geometric_map_pub = rospy.Publisher('/geometric_map',
GeometricMap,
queue_size=10,
latch=True)
self.semantic_map_pub = rospy.Publisher('/semantic_map',
SemanticMap,
queue_size=10,
latch=True)
def __init__(self):
server_ip = rospy.get_param('~overpass_server_ip')
server_port = rospy.get_param('~overpass_server_port')
ref_lat = rospy.get_param('~ref_latitude')
ref_lon = rospy.get_param('~ref_longitude')
building = rospy.get_param('~building')
global_origin = [ref_lat, ref_lon]
print(building)
rospy.loginfo("Server " + server_ip + ":" + str(server_port))
rospy.loginfo("Global origin: " + str(global_origin))
rospy.loginfo("Starting servers...")
self.osm_bridge = OSMBridge(
server_ip=server_ip,
server_port=server_port,
global_origin=global_origin,
coordinate_system="cartesian",
debug=False)
self.osm_adapter = OSMAdapter(
server_ip=server_ip,
server_port=server_port,
debug=False)
self.occ_grid_generator = OccGridGenerator(
server_ip=server_ip,
server_port=server_port,
global_origin=global_origin,
debug=False)
self.wm_query_server = SimpleActionServer('/wm_query', WMQueryAction, self._wm_query, False)
self.wm_query_server.start()
self.wm_query_callback = WMQueryCallback(self.osm_bridge)
self.osm_query_server = SimpleActionServer('/osm_query', OSMQueryAction, self._osm_query, False)
self.osm_query_server.start()
self.osm_query_callback = OSMQueryCallback(self.osm_adapter)
self.path_planner_server = SimpleActionServer('/path_planner', PathPlannerAction, self._path_planner, False)
self.path_planner = PathPlanner(self.osm_bridge)
self.path_planner.set_building(building)
self.path_planner_server.start()
self.semantic_features_server = SimpleActionServer('/semantic_features', SemanticFeaturesAction, self._semantic_features, False)
self.semantic_features_finder = SemanticFeaturesFinder(self.osm_bridge)
self.semantic_features_server.start()
self.semantic_features_callback = SemanticFeaturesCallback(self.semantic_features_finder)
self.grid_map_generator_server = SimpleActionServer('/grid_map_generator', GridMapGeneratorAction, self._grid_map_generator, False)
self.grid_map_generator_server.start()
self.nearest_wlan_server = SimpleActionServer('/nearest_wlan', NearestWLANAction, self._nearest_wlan, False)
self.nearest_wlan_finder = NearestWLANFinder(self.osm_bridge)
self.nearest_wlan_server.start()
rospy.loginfo("Servers started. Listening for queries...")
def setUp(self):
self.osm_bridge = OSMBridge()
self.osm_adapter = OSMAdapter()
class TestOSMBridge(unittest.TestCase):
def setUp(self):
self.osm_bridge = OSMBridge()
self.osm_adapter = OSMAdapter()
def test_point(self):
node_result, __, __ = self.osm_adapter.get_osm_element_by_id(
ids=[4865], data_type='node')
Point.coordinate_system = 'spherical'
p = self.osm_bridge.get_point(node_result[0])
assert p.lat is not None
assert p.lon is not None
assert p.x is not None
assert p.y is not None
# point by id
p = self.osm_bridge.get_point(4865)
assert p.id == 4865
def test_shape(self):
# point by id
s = self.osm_bridge.get_shape(1021)
assert len(s.points) > 0
def test_get_feature_by_id(self):
f = self.osm_bridge.get_feature(4865)
self.assertEqual(f.id, 4865)
def test_get_side_by_id(self):
self.assertRaises(Exception, self.osm_bridge.get_side, 99999)
def test_get_door_by_id(self):
d = self.osm_bridge.get_door(161)
self.assertEqual(d.id, 161)
assert d.geometry is not None
assert d.topology is not None
assert d.sides is None
def test_get_door_by_name(self):
d = self.osm_bridge.get_door('AMK_B_L-1_C2_Door1')
self.assertEqual(d.id, 161)
assert d.geometry is not None
assert d.topology is not None
assert d.sides is None
def test_get_door_from_point(self):
node_result, __, __ = self.osm_adapter.get_osm_element_by_id(
ids=[4978], data_type='node')
Point.coordinate_system = 'cartesian'
Point.global_origin = [50.1363485, 8.6474024]
p = Point(node_result[0])
d = self.osm_bridge.get_door(p)
self.assertEqual(d.id, 150)
def test_get_wall_by_id(self):
self.assertRaises(Exception, self.osm_bridge.get_wall, 99999)
def test_get_local_area_by_id(self):
l = self.osm_bridge.get_local_area(173)
assert l.id == 173
assert l.geometry is not None
assert l.topology is not None
def test_get_local_area_by_name(self):
l = self.osm_bridge.get_local_area('AMK_B_L4_RoomB404_LA2')
assert l.id == 173
assert l.geometry is not None
assert l.topology is not None
def test_get_local_area_from_point(self):
node_result, __, __ = self.osm_adapter.get_osm_element_by_id(
ids=[4743], data_type='node')
Point.coordinate_system = 'cartesian'
Point.global_origin = [50.1363485, 8.6474024]
p = Point(node_result[0])
l = self.osm_bridge.get_local_area(p)
self.assertEqual(l.id, 27)
def test_get_connection_by_id(self):
c = self.osm_bridge.get_connection(1199)
assert c.id == 1199
assert len(c.points) > 0
def test_get_room_by_id(self):
r = self.osm_bridge.get_room(22)
assert r.id == 22
assert r.walls is None
assert r.doors is not None
assert r.local_areas is not None
assert r.connections is not None
assert r.geometry is not None
assert r.topology is not None
def test_get_room_by_ref(self):
r = self.osm_bridge.get_room('AMK_B_L4_RoomB401')
assert r.id == 22
assert r.walls is None
assert r.doors is not None
assert r.local_areas is not None
assert r.connections is not None
assert r.geometry is not None
assert r.topology is not None
def test_get_room_from_point(self):
node_result, __, __ = self.osm_adapter.get_osm_element_by_id(
ids=[4678], data_type='node')
Point.coordinate_system = 'cartesian'
Point.global_origin = [50.1363485, 8.6474024]
p = Point(node_result[0])
r = self.osm_bridge.get_room(p)
self.assertEqual(r.id, 22)
def test_get_corridor_by_id(self):
r = self.osm_bridge.get_corridor(140)
assert r.id == 140
assert r.walls is None
assert r.doors is None
assert r.local_areas is not None
assert r.connections is not None
assert r.geometry is not None
assert r.topology is not None
def test_get_corridor_by_ref(self):
r = self.osm_bridge.get_corridor('AMK_B_L-1_C14')
assert r.id == 140
assert r.walls is None
assert r.doors is None
assert r.local_areas is not None
assert r.connections is not None
assert r.geometry is not None
assert r.topology is not None
def test_get_corridor_from_point(self):
node_result, __, __ = self.osm_adapter.get_osm_element_by_id(
ids=[4666], data_type='node')
Point.coordinate_system = 'cartesian'
Point.global_origin = [50.1363485, 8.6474024]
p = Point(node_result[0])
c = self.osm_bridge.get_corridor(p)
self.assertEqual(c.id, 14)
def test_get_elevator_by_id(self):
e = self.osm_bridge.get_elevator(5)
assert e.id == 5
assert e.walls is None
assert e.doors is None
assert e.local_areas is not None
assert e.connections is not None
assert e.geometry is not None
assert e.topology is not None
def test_get_elevator_by_name(self):
e = self.osm_bridge.get_elevator('AMK_B_E1')
assert e.id == 5
assert e.walls is None
assert e.doors is None
assert e.local_areas is not None
assert e.connections is not None
assert e.geometry is not None
assert e.topology is not None
def test_search_by_scope_for_elevator(self):
e = self.osm_bridge.get_elevator('AMK_B_E1')
assert e.id == 5
la = e.local_area('AMK_B_E1_LA1')
assert la.id == 163
def test_get_floor(self):
f = self.osm_bridge.get_floor(164)
assert f.id == 164
assert f.walls is None
assert f.corridors is not None
assert f.rooms is not None
assert f.connections is not None
def test_get_floor_by_name(self):
f = self.osm_bridge.get_floor('AMK_L4')
assert f.id == 164
assert f.walls is None
assert f.corridors is not None
assert f.rooms is not None
assert f.connections is not None
def test_search_by_scope_for_floor(self):
f = self.osm_bridge.get_floor('AMK_L4')
r = f.room('AMK_B_L4_RoomB401')
assert r.id == 22
c = f.corridor('AMK_B_L4_C6')
assert c.id == 19
def test_get_building_by_id(self):
b = self.osm_bridge.get_building(149)
assert b.geometry is not None
assert b.id == 149
assert b.stairs is None
assert b.elevators is not None
assert b.floors is not None
def test_get_building_by_name(self):
b = self.osm_bridge.get_building('AMK')
assert b.geometry is not None
assert b.id == 149
assert b.stairs is None
assert b.elevators is not None
assert b.floors is not None
def test_search_by_scope_for_building(self):
b = self.osm_bridge.get_building('AMK')
f = b.floor('AMK_L4')
assert f.id == 164
e = b.elevator('AMK_B_E1')
assert e.id == 5
self.assertRaises(Exception, b.stair, 'AMK_B_S1')
def setUp(self):
self.osm_adapter = OSMAdapter(debug=False)
class TestOSMAdapter(unittest.TestCase):
def setUp(self):
self.osm_adapter = OSMAdapter(debug=False)
def test_get_osm_element_by_id(self):
result_node = self.osm_adapter.get_osm_element_by_id(ids=[4865],
data_type='node')
self.assertEqual(len(result_node[0]), 1)
result_way = self.osm_adapter.get_osm_element_by_id(ids=[499],
data_type='way')
self.assertEqual(len(result_way[1]), 1)
result_relation = self.osm_adapter.get_osm_element_by_id(
ids=[149], data_type='relation')
self.assertEqual(len(result_relation[2]), 1)
result_relation_role = self.osm_adapter.get_osm_element_by_id(
ids=[149], data_type='relation', role='geometry', role_type='way')
self.assertEqual(len(result_relation_role[1]), 1)
def test_get_osm_element_by_multiple_id(self):
nodes, __, __ = self.osm_adapter.get_osm_element_by_id(
ids=[4866, 4865, 4864], data_type='node')
self.assertEqual(len(nodes), 3)
self.assertEqual(nodes[0].id, 4866)
self.assertEqual(nodes[1].id, 4865)
self.assertEqual(nodes[2].id, 4864)
__, way, __ = self.osm_adapter.get_osm_element_by_id(
ids=[500, 501, 499], data_type='way')
self.assertEqual(len(way), 3)
self.assertEqual(way[0].id, 500)
self.assertEqual(way[1].id, 501)
self.assertEqual(way[2].id, 499)
__, __, relation = self.osm_adapter.get_osm_element_by_id(
ids=[149, 151, 150], data_type='relation')
self.assertEqual(len(relation), 3)
self.assertEqual(relation[0].id, 149)
self.assertEqual(relation[1].id, 151)
self.assertEqual(relation[2].id, 150)
__, way, __ = self.osm_adapter.get_osm_element_by_id(
ids=[150, 151, 149],
data_type='relation',
role='geometry',
role_type='way')
self.assertEqual(len(way), 3)
self.assertEqual(way[0].id, 1309)
self.assertEqual(way[1].id, 1312)
self.assertEqual(way[2].id, 1352)
def test_search_by_tag(self):
result_node = self.osm_adapter.search_by_tag(data_type='node',
key='highway',
value='elevator')
self.assertTrue(len(result_node[0]) > 0)
result_way = self.osm_adapter.search_by_tag(data_type='way',
key='highway',
value='footway')
self.assertTrue(len(result_way[1]) > 0)
""" search with multiple tags"""
__, result_ways, __ = self.osm_adapter.search_by_tag(data_type='way',
key_val_dict={
'level': '-1',
'indoor':
'wall'
})
self.assertEqual(len(result_ways), 198)
result_relation = self.osm_adapter.search_by_tag(data_type='relation',
key='type',
value='building')
self.assertTrue(len(result_relation[2]) > 0)
def test_get_parent(self):
__, __, result_relation = self.osm_adapter.get_parent(4865,
'node',
'topology',
role_type='',
role='')
self.assertTrue(len(result_relation) > 0)
__, result_way, __ = self.osm_adapter.get_parent(
4865, 'node', 'topology', 'way', 'geometry')
self.assertTrue(len(result_way) > 0)
def test_overpass_connection_success(self):
osm_adapter = OSMAdapter()
status = osm_adapter.test_overpass_connection()
self.assertTrue(status)