def add_edge_to_map(descriptor_link):
"""Add two vertices and an edge and assign the segment"""
iface = MapAgentInterface(start=False)
map_agent = rospy.ServiceProxy(iface.action_service_name,
ActOnMap)
rospy.loginfo('Waiting for service')
map_agent.wait_for_service()
rospy.loginfo('MapAgent ready')
edge = LamaObject()
edge.type = edge.EDGE
edge.references = [-1, -1]
edge_response = map_agent(object=edge, action=ActOnMapRequest.PUSH_EDGE)
if not edge_response.objects:
rospy.logerr('Database error')
return
edge_id = edge_response.objects[0].id
rospy.loginfo('edge {} added'.format(edge_id))
# Assign segment.
map_action = ActOnMapRequest()
map_action.action = map_action.ASSIGN_DESCRIPTOR_EDGE
map_action.object.id = edge_id
map_action.descriptor_id = descriptor_link.descriptor_id
map_action.interface_name = descriptor_link.interface_name
map_agent(map_action)
rospy.loginfo('segment {} assigned to edge {}'.format(
descriptor_link.descriptor_id, edge_id))
def _lama_object_from_query_result(self, result):
lama_object = LamaObject()
for attr in self.direct_attributes:
setattr(lama_object, attr, result[attr])
lama_object.references[0] = result['v0']
lama_object.references[1] = result['v1']
return lama_object
def test003_pull_edge(self):
"""Test ActOnMap service PULL_EDGE"""
req = LamaObject()
req.id = edge1.id
response = self.map_agent(object=req, action=ActOnMapRequest.PULL_EDGE)
self.assertEqual(len(response.objects), 1)
# Workaround a strange bug, where msg.object.references is a tuple.
response.objects[0].references = list(response.objects[0].references)
self.assertObjectEqual(response.objects[0], edge1)
def get_edges_with_vertices(v0, v1):
"""Return the list of edges from v0 to v1, as LamaObject
Return the list of edges from v0 to v1, as LamaObject. Return None on
service error.
"""
edge = LamaObject()
edge.type = edge.EDGE
edge.references = [v0, v1]
return _map_agent.get_edge_list(edge)
def test003_pull_edge_name(self):
""" test ActOnMap service GET_EDGE_LIST according name"""
req = LamaObject()
req.name = edge1.name
response = self.map_agent(object=req,
action=ActOnMapRequest.GET_EDGE_LIST)
self.assertNonEmpty(response.objects)
self.assertIn(edge1.id, [o.id for o in response.objects])
for obj in response.objects:
# Workaround a strange bug, where references is a tuple.
obj.references = list(obj.references)
self.assertNameEqual(obj, edge1)
def test003_pull_edge_id_in_world(self):
"""test ActOnMap service GET_EDGE_LIST according id in world"""
req = LamaObject()
req.id_in_world = edge1.id_in_world
response = self.map_agent(object=req,
action=ActOnMapRequest.GET_EDGE_LIST)
self.assertNonEmpty(response.objects)
self.assertIn(edge1.id, [o.id for o in response.objects])
for obj in response.objects:
# Workaround a strange bug, where references is a tuple.
obj.references = list(obj.references)
self.assertIdInWorldEqual(obj, vertex1)
def test002_pull_vertex_name(self):
"""Test ActOnMap service GET_VERTEX_LIST according to name"""
req = LamaObject()
req.name = vertex1.name
response = self.map_agent(object=req,
action=ActOnMapRequest.GET_VERTEX_LIST)
self.assertNonEmpty(response.objects)
self.assertIn(vertex1.id, [o.id for o in response.objects])
for obj in response.objects:
# Workaround a strange bug, where references is a tuple.
obj.references = list(obj.references)
self.assertNameEqual(obj, vertex1)
def test003_pull_edge_stop_vertex(self):
"""test ActOnMap service GET_EDGE_LIST according to stop vertex"""
req = LamaObject()
req.references[1] = edge1.references[1]
response = self.map_agent(object=req,
action=ActOnMapRequest.GET_EDGE_LIST)
self.assertNonEmpty(response.objects)
self.assertIn(edge1.id, [o.id for o in response.objects])
for obj in response.objects:
self.assertEqual(
obj.references[1], edge1.references[1],
'wrong start vertex, got {}, expected {}'.format(
obj.references[1], edge1.references[1]))
def test003_get_outgoing_edges(self):
"""test ActOnMap service GET_OUTGOING_EDGES"""
req = LamaObject()
req.id = edge1.references[0]
response = self.map_agent(object=req,
action=ActOnMapRequest.GET_OUTGOING_EDGES)
self.assertNonEmpty(response.objects)
self.assertIn(edge1.id, [o.id for o in response.objects])
for obj in response.objects:
self.assertEqual(
obj.references[0], edge1.references[0],
'wrong start vertex, got {}, expected {}'.format(
obj.references[0], edge1.references[0]))
def get_descriptors(object_id, interface, getter):
"""Retrieve the descriptors associated with LamaObject with id object_id
Return a list of descriptors associated with getter.
Parameters
----------
- object_id: int, LamaObject's id
- interface: str, interface name associated with getter
- getter: ROS ServiceProxy for a ROS message type
"""
map_action = ActOnMapRequest()
map_action.action = map_action.GET_DESCRIPTOR_LINKS
lama_object = LamaObject()
lama_object.id = object_id
map_action.object = lama_object
map_action.interface_name = interface
response = _map_agent.proxy(map_action)
descriptors = []
for descriptor_link in response.descriptor_links:
getter_response = getter(descriptor_link.descriptor_id)
descriptors.append(getter_response.descriptor)
return descriptors
def _check_md5sum(self):
"""Check that current implementation is compatible with LamaObject"""
lama_object = LamaObject()
if lama_object._md5sum != _expected_lama_object_md5sum:
raise rospy.ROSException('CoreDBInterface incompatible ' +
'with current LamaObject implementation')
def __init__(self, interface_name=None, descriptor_table_name=None,
start=False):
"""Build the map interface for LamaObject
Parameters
----------
- interface_name: string, name of the table containing LamaObject
messages (vertices and edges). Defaults to 'lama_objects'.
- descriptor_table_name: string, name of the table for DescriptorLink
messages. Defaults to 'lama_descriptor_links'.
"""
self._check_md5sum()
self.direct_attributes = ['id', 'id_in_world', 'name', 'emitter_id',
'emitter_name', 'type']
if not interface_name:
interface_name = _default_core_table_name
if not descriptor_table_name:
descriptor_table_name = _default_descriptor_table_name
self.descriptor_table_name = descriptor_table_name
get_srv_type = 'lama_msgs/GetLamaObject'
set_srv_type = 'lama_msgs/SetLamaObject'
super(CoreDBInterface, self).__init__(interface_name,
get_srv_type, set_srv_type,
start=start)
# Add the "unvisited" vertex. Edge for which the outoing vertex is not
# visited yet have reference[1] == unvisited_vertex.id.
unvisited_vertex = LamaObject()
unvisited_vertex.id = -1
unvisited_vertex.name = 'unvisited'
unvisited_vertex.type = LamaObject.VERTEX
self.set_lama_object(unvisited_vertex)
# Add the "unknown" edge. This is used to indicate that the robot
# position is unknown because we didn't recognized any vertex yet.
unknown_edge = LamaObject()
unknown_edge.id = -2
unknown_edge.name = 'unknown'
unknown_edge.type = LamaObject.EDGE
unknown_edge.references[0] = unvisited_vertex.id
unknown_edge.references[1] = unvisited_vertex.id
self.set_lama_object(unknown_edge)
# Add the "undefined" vertex. This is to ensure that automatically
# generated ids (primary keys) are greater than 0.
undefined_vertex = LamaObject()
undefined_vertex.id = 0
undefined_vertex.name = 'undefined'
undefined_vertex.type = LamaObject.VERTEX
self.set_lama_object(undefined_vertex)
#!/usr/bin/python # -*- coding: utf-8 -*- # Unit tests for core_interface.py import unittest import rospy from lama_interfaces.srv import ActOnMap from lama_interfaces.srv import ActOnMapRequest from lama_msgs.msg import LamaObject vertex1 = LamaObject() vertex1.id_in_world = 101 vertex1.name = 'vertex1' vertex1.type = LamaObject.VERTEX vertex2 = LamaObject() vertex2.id_in_world = 102 vertex2.name = 'vertex2' vertex2.type = LamaObject.VERTEX vertex3 = LamaObject() vertex3.id_in_world = 103 vertex3.name = 'vertex3' vertex3.type = LamaObject.VERTEX vertex3a = LamaObject() vertex3a.id_in_world = 104 vertex3a.name = 'vertex3' vertex3a.type = LamaObject.VERTEX