Go to the documentation of this file.00001 import copy
00002
00003 import rospy
00004 import roslib.message
00005
00006 from lama_msgs.msg import LamaObject
00007 from lama_msgs.srv import GetInterfaceInfo
00008 from lama_msgs.srv import GetInterfaceInfoResponse
00009
00010 from lama_interfaces.core_interface import CoreDBInterface
00011 from lama_interfaces.map_agent import map_agent_name
00012 from lama_interfaces.srv import ActOnMap
00013 from lama_interfaces.srv import ActOnMapRequest
00014 from lama_interfaces.srv import ActOnMapResponse
00015
00016
00017 class LocalMapAgent(object):
00018 """Define callbacks for ActOnMap and possibly start the map agent service
00019
00020 The class directly accesses the database.
00021 """
00022 def __init__(self, start=False):
00023 action_srv_type = 'lama_interfaces/ActOnMap'
00024 srv_action_class = roslib.message.get_service_class(action_srv_type)
00025
00026 self.action_service_name = map_agent_name()
00027 self.action_service_class = srv_action_class
00028 if start:
00029 self.map_agent = rospy.Service(self.action_service_name,
00030 self.action_service_class,
00031 self.action_callback)
00032
00033 rospy.Service('get_interface_info',
00034 GetInterfaceInfo,
00035 self._get_interface_info_callback)
00036 else:
00037 self.map_agent = None
00038 self.proxy = rospy.ServiceProxy(self.action_service_name, ActOnMap)
00039
00040 self.core_iface = CoreDBInterface(start=start)
00041
00042 def get_lama_object_list(self, lama_object):
00043 """Retrieve all elements that match the search criteria
00044
00045 Search criteria are attributes of lama_object with non-default values
00046 (defaults are 0 or '').
00047 Return a list of LamaObject, or an empty list of no LamaObject matches.
00048
00049 Parameters
00050 ----------
00051 - lama_object: an instance of LamaObject
00052 """
00053
00054 if lama_object.type == LamaObject.VERTEX:
00055 lama_object.references = [0, 0]
00056
00057 coretable = self.core_iface.core_table
00058 query = coretable.select()
00059
00060 query = query.where(coretable.c['id'] != 0)
00061 for attr in self.core_iface.direct_attributes:
00062 v = getattr(lama_object, attr)
00063 if v:
00064
00065
00066 query = query.where(coretable.c[attr] == v)
00067 if lama_object.references[0]:
00068 query = query.where(coretable.c['v0'] == lama_object.references[0])
00069 if lama_object.references[1]:
00070 query = query.where(coretable.c['v1'] == lama_object.references[1])
00071 rospy.logdebug('SQL query: {}'.format(query))
00072
00073 connection = self.core_iface.engine.connect()
00074 with connection.begin():
00075 results = connection.execute(query).fetchall()
00076 connection.close()
00077 if not results:
00078 return []
00079 objects = []
00080 for result in results:
00081 lama_object = self.core_iface._lama_object_from_query_result(result)
00082 objects.append(lama_object)
00083 return objects
00084
00085 def action_callback(self, msg):
00086 """Callback of ActOnMap service
00087
00088 Return an instance of ActOnMap response.
00089
00090 Parameters
00091 ----------
00092 - msg: an instance of ActOnMap request.
00093 """
00094
00095 if ((msg.action == msg.PUSH_VERTEX) and
00096 (msg.object.type not in [0, msg.object.VERTEX])):
00097 raise rospy.ServiceException(
00098 'Action PUSH_VERTEX, LamaObject is not a vertex')
00099 if ((msg.action == msg.PUSH_EDGE) and
00100 (msg.object.type not in [0, msg.object.EDGE])):
00101 raise rospy.ServiceException(
00102 'Action PUSH_EDGE, LamaObject is not an edge')
00103
00104
00105 if msg.action == msg.PUSH_EDGE:
00106 msg.object.type = msg.object.EDGE
00107 if msg.action == msg.PUSH_VERTEX:
00108 msg.object.type = msg.object.VERTEX
00109
00110 callbacks = {
00111 msg.PUSH_VERTEX: self.push_lama_object,
00112 msg.PULL_VERTEX: self.pull_lama_object,
00113 msg.ASSIGN_DESCRIPTOR_VERTEX: (
00114 self.assign_descriptor_to_lama_object),
00115 msg.PUSH_EDGE: self.push_lama_object,
00116 msg.PULL_EDGE: self.pull_lama_object,
00117 msg.ASSIGN_DESCRIPTOR_EDGE: (
00118 self.assign_descriptor_to_lama_object),
00119 msg.GET_VERTEX_LIST: self.get_vertex_list,
00120 msg.GET_EDGE_LIST: self.get_edge_list,
00121 msg.GET_DESCRIPTOR_LINKS: self.get_descriptor_links,
00122 msg.GET_NEIGHBOR_VERTICES: self.get_neighbor_vertices,
00123 msg.GET_OUTGOING_EDGES: self.get_outgoing_edges,
00124 }
00125 if msg.action not in callbacks:
00126 raise rospy.ServiceException('Action {} not implemented'.format(
00127 msg.action))
00128 callback = callbacks[msg.action]
00129 response = callback(msg)
00130 return response
00131
00132 def _get_interface_info_callback(self, msg):
00133 response = GetInterfaceInfoResponse()
00134 response.interface_info = self.core_iface.get_interface_info(
00135 msg.interface_name)
00136 if response.interface_info is None:
00137 return
00138 return response
00139
00140 def push_lama_object(self, msg):
00141 """Add a LaMa object to the database
00142
00143 Callback for PUSH_VERTEX and PUSH_EDGE.
00144 Return an instance of ActOnMap response.
00145
00146 Parameters
00147 ----------
00148 - msg: an instance of ActOnMap request.
00149 """
00150 lama_object = copy.copy(msg.object)
00151 lama_object.id = self.core_iface.set_lama_object(lama_object)
00152 response = ActOnMapResponse()
00153 response.objects.append(lama_object)
00154 return response
00155
00156 def pull_lama_object(self, msg):
00157 """Retrieve a LaMa object from the database
00158
00159 Callback for PULL_VERTEX and PULL_EDGE.
00160 The object id is given in msg.object.id.
00161 Return an instance of ActOnMap response. The field descriptor_links will
00162 be filled with all DescriptorLink associated with this LamaObject.
00163 An error is raised if more LaMa objects correspond to the search
00164 criteria.
00165
00166 Parameters
00167 ----------
00168 - msg: an instance of ActOnMap request.
00169 """
00170 response = ActOnMapResponse()
00171 id_ = msg.object.id
00172 response.objects.append(self.core_iface.get_lama_object(id_))
00173 get_desc_links = self.core_iface.get_descriptor_links
00174 response.descriptor_links = get_desc_links(id_)
00175 return response
00176
00177 def assign_descriptor_to_lama_object(self, msg):
00178 """Add a descriptor to a vertex
00179
00180 Callback for ASSIGN_DESCRIPTOR_VERTEX and ASSIGN_DESCRIPTOR_EDGE.
00181 Return an instance of ActOnMap response.
00182
00183 Parameters
00184 ----------
00185 - msg: an instance of ActOnMap request.
00186 """
00187
00188 object_id = msg.object.id
00189 core_table = self.core_iface.core_table
00190 query = core_table.select(
00191 whereclause=(core_table.c.id == object_id))
00192 connection = self.core_iface.engine.connect()
00193 with connection.begin():
00194 result = connection.execute(query).fetchone()
00195 connection.close()
00196 if not result:
00197 err = 'No lama object with id {} in database table {}'.format(
00198 object_id, core_table.name)
00199 raise rospy.ServiceException(err)
00200
00201
00202 if not msg.interface_name:
00203 raise rospy.ServiceException('Missing interface name')
00204 table_name = msg.interface_name
00205 if not self.core_iface.has_table(table_name):
00206 err = 'No interface "{}" in the database'.format(
00207 msg.interface_name)
00208 raise rospy.ServiceException(err)
00209 table = self.core_iface.metadata.tables[table_name]
00210 desc_id = msg.descriptor_id
00211 query = table.select(
00212 whereclause=(table.c.id == desc_id))
00213 connection = self.core_iface.engine.connect()
00214 with connection.begin():
00215 result = connection.execute(query).fetchone()
00216 connection.close()
00217 if not result:
00218 err = 'No descriptor with id {} in database table {}'.format(
00219 desc_id, table.name)
00220 raise rospy.ServiceException(err)
00221
00222
00223 time = rospy.Time.now()
00224 insert_args = {
00225 'object_id': object_id,
00226 'descriptor_id': desc_id,
00227 'interface_name': table_name,
00228 'timestamp_secs': time.secs,
00229 'timestamp_nsecs': time.nsecs,
00230 }
00231 connection = self.core_iface.engine.connect()
00232 with connection.begin():
00233 connection.execute(self.core_iface.descriptor_table.insert(),
00234 insert_args)
00235 connection.close()
00236
00237 response = ActOnMapResponse()
00238 return response
00239
00240 def get_vertex_list(self, msg):
00241 """Retrieve all vertices from the database
00242
00243 Callback for GET_VERTEX_LIST.
00244 Return an instance of ActOnMap response.
00245
00246 Parameters
00247 ----------
00248 - msg: an instance of ActOnMap request.
00249 """
00250 msg.object.type = LamaObject.VERTEX
00251 response = ActOnMapResponse()
00252 response.objects = self.get_lama_object_list(msg.object)
00253 return response
00254
00255 def get_edge_list(self, msg):
00256 """Retrieve edges from the database
00257
00258 Callback for GET_EDGE_LIST.
00259 Retrieved all edges from the database that correspond to the search
00260 criteria given in msg.object.
00261 Search criteria are attributes with non-default values (0 or '').
00262 Return an instance of ActOnMap response.
00263
00264 Parameters
00265 ----------
00266 - msg: an instance of ActOnMap request.
00267 """
00268 msg.object.type = LamaObject.EDGE
00269 response = ActOnMapResponse()
00270 response.objects = self.get_lama_object_list(msg.object)
00271 return response
00272
00273 def get_descriptor_links(self, msg):
00274 """Retrieve DescriptorLink associated with a LamaObject and an interface
00275
00276 Callback for GET_DESCRIPTOR_LINKS.
00277 Return an instance of ActOnMap response.
00278
00279 Parameters
00280 ----------
00281 - msg: an instance of ActOnMap request.
00282 """
00283 response = ActOnMapResponse()
00284 response.objects.append(msg.object)
00285 get_desc_links = self.core_iface.get_descriptor_links
00286 response.descriptor_links = get_desc_links(msg.object.id,
00287 msg.interface_name)
00288 return response
00289
00290 def get_neighbor_vertices(self, msg):
00291 """Retrieve all neighbor vertices from the database
00292
00293 Callback for GET_NEIGHBOR_VERTICES.
00294 Return an instance of ActOnMap response.
00295
00296 Parameters
00297 ----------
00298 - msg: an instance of ActOnMap request.
00299 """
00300
00301 rospy.logerr('GET_NEIGHBOR_VERTICES not implemented')
00302 response = ActOnMapResponse()
00303 return response
00304
00305 def get_outgoing_edges(self, msg):
00306 """Retrieve edges starting at a given vertex
00307
00308 Callback for GET_OUTGOING_EDGES.
00309 Return an instance of ActOnMap response containing edges (instances
00310 of LamaObject) starting at the given vertex.
00311 This is syntactic sugar because the functionality can be obtained with
00312 GET_EDGE_LIST.
00313
00314 Parameters
00315 ----------
00316 - msg: an instance of ActOnMapRequest.
00317 """
00318 msg_get_edges = ActOnMapRequest()
00319 msg_get_edges.object.type = msg_get_edges.object.EDGE
00320 msg_get_edges.object.references[0] = msg.object.id
00321 return self.get_edge_list(msg_get_edges)