database_server emits a message once database is modified

Author: fcladera

mocha_core/CMakeLists.txt

@@ -22,6 +22,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
   "msg/ClientStats.msg"
   "msg/HeaderPub.msg"
   "msg/SMState.msg"
+  "msg/DatabaseCB.msg"
   "srv/AddUpdateDB.srv"
   "srv/GetDataHeaderDB.srv"
   "srv/SelectDB.srv"

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, ros_node=None):
+    def __init__(self, sample_db=None, ros_node=None, insertion_cb=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
@@ -91,8 +91,11 @@ def __init__(self, sample_db=None, ros_node=None):
             self.db = sample_db
         else:
             self.db = {}
+        if insertion_cb is not None:
+            assert callable(insertion_cb)
         self.lock = threading.Lock()
         self.ros_node = ros_node
+        self.insertion_cb = insertion_cb
 
     def add_modify_data(self, dbm):
         """ Insert new stuff into the db. Use the header as message index """
@@ -113,6 +116,9 @@ def add_modify_data(self, dbm):
                 raise Exception("database: Header Collision Detected")
         self.db[dbm.robot_id][dbm.topic_id][dbm.header] = dbm
         self.lock.release()
+        # Callback on insertion of new data
+        if self.insertion_cb is not None:
+            self.insertion_cb(dbm.robot_id, dbm.topic_id, dbm.ts)
         return dbm.header
 
     def get_header_list(self, filter_robot_id=None, filter_latest=None):

mocha_core/mocha_core/database_server.py

@@ -10,6 +10,7 @@
 import mocha_core.database as database
 import pdb
 import mocha_core.database_utils as du
+import mocha_core.msg
 
 from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy, DurabilityPolicy
 
@@ -52,8 +53,22 @@ 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"]]
 
+        # Publish a message every time that the database is modified
+        pub_database = self.ros_node.create_publisher(
+            mocha_core.msg.DatabaseCB,
+            f"{self.ros_node_name}/database_cb",
+            10
+        )
+        def database_cb(robot_id, topic_id, ts):
+            database_cb_msg = mocha_core.msg.DatabaseCB()
+            database_cb_msg.header.stamp = ts.to_msg()
+            database_cb_msg.header.frame_id = self.robot_name
+            database_cb_msg.robot_id = robot_id
+            database_cb_msg.topic_id = topic_id
+            pub_database.publish(database_cb_msg)
+
         # Create the empty database with lock
-        self.dbl = database.DBwLock(ros_node=self.ros_node)
+        self.dbl = database.DBwLock(ros_node=self.ros_node, insertion_cb=database_cb)
 
         # Get current robot number
         self.robot_number = du.get_robot_id_from_name(self.robot_configs,

mocha_core/msg/DatabaseCB.msg

@@ -0,0 +1,3 @@
+std_msgs/Header header
+uint16  robot_id
+uint16  topic_id

mocha_core/test/test_database_server.py

@@ -23,9 +23,7 @@
 import mocha_core.database_server as ds
 import mocha_core.database_utils as du
 from mocha_core.srv import AddUpdateDB, GetDataHeaderDB, SelectDB
-
-
-import mocha_core.srv
+from mocha_core.msg import DatabaseCB
 
 
 class Database_server_test(Node):
@@ -51,11 +49,18 @@ def __init__(self):
         while not self.get_data_header_db_cli.wait_for_service(timeout_sec=1.0):
             logger.debug("Waiting for get_data_header_db")
         self.select_db_cli = self.create_client(SelectDB,
-                                                    "/test_database_server/select_db",
+                                                "/test_database_server/select_db",
                                                 qos_profile=ds.DatabaseServer.QOS_PROFILE)
         while not self.select_db_cli.wait_for_service(timeout_sec=1.0):
             logger.debug("Waiting for get_data_header_db")
 
+        # Subscribe to the answers from the database
+        def callback(data):
+            self.data_answer = data
+        self.database_cb_sub = self.create_subscription(DatabaseCB,
+                                                        "/test_database_server/database_cb",
+                                                        callback, 10)
+
     def recorder_async(self, loops_per_thread, robot_configs, topic_configs, robot_name):
         # Get a random ros time
         tid = du.get_topic_id_from_name(
@@ -82,7 +87,7 @@ def recorder_async(self, loops_per_thread, robot_configs, topic_configs, robot_n
             ).to_msg()
             try:
                 # Create request and call service method directly
-                req = mocha_core.srv.AddUpdateDB.Request()
+                req = AddUpdateDB.Request()
                 req.topic_id = tid
                 req.timestamp = timestamp
                 req.msg_content = serialized_msg
@@ -174,7 +179,7 @@ def test_add_retrieve_single_msg(self):
 
         try:
             # Create request and call service method directly
-            req = mocha_core.srv.AddUpdateDB.Request()
+            req = AddUpdateDB.Request()
             req.topic_id = tid
             req.timestamp = rclpy.clock.Clock().now().to_msg()
             req.msg_content = serialized_msg
@@ -191,7 +196,7 @@ def test_add_retrieve_single_msg(self):
         # Request the same hash through service
         try:
             # Create request and call service method directly
-            req = mocha_core.srv.GetDataHeaderDB.Request()
+            req = GetDataHeaderDB.Request()
             req.msg_header = answ_header
             future = client_node.get_data_header_db_cli.call_async(req)
             rclpy.spin_until_future_complete(client_node, future)
@@ -207,9 +212,12 @@ def test_add_retrieve_single_msg(self):
 
         self.assertEqual(tid, ans_topic_id)
         self.assertEqual(ans_data, sample_msg)
-        # print("Received topic:", ans_topic_name)
-        # print("ROS msg:", ans_data)
-        # print("Timestamp:", ans_ts)
+
+        # We will now test the callback from the database when a new message is
+        # published
+        self.assertTrue(client_node.data_answer is not None)
+        self.assertEqual(client_node.data_answer.topic_id, tid)
+        self.assertEqual(client_node.data_answer.header.frame_id, self.robot_name)
 
     def test_add_select_robot(self):
         # Create a single client
@@ -237,7 +245,7 @@ def test_add_select_robot(self):
             serialized_msg = du.serialize_ros_msg(sample_msg)
             try:
                 # Create request and call service method directly
-                req = mocha_core.srv.AddUpdateDB.Request()
+                req = AddUpdateDB.Request()
                 req.topic_id = tid
                 req.timestamp = rclpy.clock.Clock().now().to_msg()
                 req.msg_content = serialized_msg
@@ -254,7 +262,7 @@ def test_add_select_robot(self):
         try:
             robot_id = du.get_robot_id_from_name(self.robot_configs, self.robot_name)
             # Create request and call service method directly
-            req = mocha_core.srv.SelectDB.Request()
+            req = SelectDB.Request()
             req.robot_id = robot_id
             req.topic_id = 0  # Changed from None to 0 since it's uint8
             req.ts_limit = rclpy.clock.Clock().now().to_msg()
@@ -299,7 +307,7 @@ def test_insert_storm(self):
         try:
             robot_id = du.get_robot_id_from_name(self.robot_configs, self.robot_name)
             # Create request and call service method directly
-            req = mocha_core.srv.SelectDB.Request()
+            req = SelectDB.Request()
             req.robot_id = robot_id
             req.topic_id = 0  # Changed from None to 0 since it's uint8
             req.ts_limit = rclpy.clock.Clock().now().to_msg()
@@ -315,4 +323,4 @@ def test_insert_storm(self):
 
 
 if __name__ == "__main__":
-    unittest.main()
+    unittest.main(failfast=True)