Publish database header in database_cb, makes topic_publisher easier

Author: fcladera

mocha_core/mocha_core/database.py

@@ -118,7 +118,7 @@ def add_modify_data(self, 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)
+            self.insertion_cb(dbm.robot_id, dbm.topic_id, dbm.ts, dbm.header)
         return dbm.header
 
     def get_header_list(self, filter_robot_id=None, filter_latest=None):

mocha_core/mocha_core/database_server.py

@@ -59,12 +59,13 @@ def __init__(self, robot_configs, topic_configs, robot_name, ros_node):
             f"{self.ros_node_name}/database_cb",
             10
         )
-        def database_cb(robot_id, topic_id, ts):
+        def database_cb(robot_id, topic_id, ts, header):
             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
+            database_cb_msg.msgheader = header
             pub_database.publish(database_cb_msg)
 
         # Create the empty database with lock

mocha_core/msg/DatabaseCB.msg

@@ -1,3 +1,4 @@
 std_msgs/Header header
 uint16  robot_id
 uint16  topic_id
+uint8[] msgheader

mocha_core/test/test_database_server.py

@@ -218,6 +218,7 @@ def test_add_retrieve_single_msg(self):
         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)
+        self.assertEqual(client_node.data_answer.msgheader, answ_header)
 
     def test_add_select_robot(self):
         # Create a single client