Database can now report collisions

Author: fcladera

mocha_core/mocha_core/database.py

@@ -82,7 +82,7 @@ class DBwLock():
 
     sample_db may be a valid dictionary that can be preloaded into
     the object (useful for debugging, see sample_db.py) """
-    def __init__(self, sample_db=None):
+    def __init__(self, sample_db=None, ros_node=None):
         if sample_db is not None:
             assert isinstance(sample_db, dict)
             # For ROS2, we can't deepcopy Time objects, so we'll just assign directly
@@ -92,6 +92,7 @@ def __init__(self, sample_db=None):
         else:
             self.db = {}
         self.lock = threading.Lock()
+        self.ros_node = ros_node
 
     def add_modify_data(self, dbm):
         """ Insert new stuff into the db. Use the header as message index """
@@ -106,7 +107,10 @@ def add_modify_data(self, dbm):
         if dbm.topic_id not in self.db[dbm.robot_id]:
             self.db[dbm.robot_id][dbm.topic_id] = {}
         if dbm.header in self.db[dbm.robot_id][dbm.topic_id]:
-            raise Exception("database: Header Collision Detected")
+            if self.ros_node is not None:
+                self.ros_node.get_logger().error("Database - Header Collision Detected. " + "You may want to decrease your publish rate into MOCHA")
+            else:
+                raise Exception("database: Header Collision Detected")
         self.db[dbm.robot_id][dbm.topic_id][dbm.header] = dbm
         self.lock.release()
         return dbm.header

mocha_core/mocha_core/database_server.py

@@ -53,7 +53,7 @@ def __init__(self, robot_configs, topic_configs, robot_name, ros_node):
         self.topic_list = self.topic_configs[self.robot_configs[self.robot_name]["node-type"]]
 
         # Create the empty database with lock
-        self.dbl = database.DBwLock()
+        self.dbl = database.DBwLock(ros_node=self.ros_node)
 
         # Get current robot number
         self.robot_number = du.get_robot_id_from_name(self.robot_configs,
@@ -137,9 +137,6 @@ def get_data_hash_db_service_cb(self, req, response):
     def select_db_service_cb(self, req, response):
         # TODO Implement filtering
 
-        # Note: robot_id and topic_id are uint8 in ROS2, so they can't be None
-        # We can add range validation if needed, but for now just proceed
-
         list_headers = self.dbl.get_header_list(req.robot_id)
 
         response.headers = du.serialize_headers(list_headers)