Mostly completed migration of database_server.py

Author: fcladera

mocha_core/CMakeLists.txt

@@ -62,6 +62,7 @@ if(BUILD_TESTING)
   ament_add_pytest_test(test_database_utils test/test_database_utils.py)
   ament_add_pytest_test(test_zmq_comm_node test/test_zmq_comm_node.py)
   ament_add_pytest_test(test_synchronize_channel test/test_synchronize_channel.py)
+  ament_add_pytest_test(test_database_server test/test_database_server.py)
 endif()
 
 ament_package()

mocha_core/mocha_core/database_server.py

@@ -2,7 +2,9 @@
 
 import os
 import threading
-import rospy
+import rclpy
+import rclpy.logging
+import rclpy.time
 import mocha_core.srv
 import database
 import pdb
@@ -15,21 +17,28 @@ class DatabaseServer:
 
     Please see the list of services in the srv folder
     """
-    def __init__(self, robot_configs, topic_configs):
+    def __init__(self, robot_configs, topic_configs, node=None):
         # Check input topics
         assert isinstance(robot_configs, dict)
         assert isinstance(topic_configs, dict)
 
         self.robot_configs = robot_configs
         self.topic_configs = topic_configs
-
-        # Get current robot name from params
-        self.robot_name = rospy.get_param('~robot_name')
+        
+        # Store or create the ROS2 node
+        self.node = node
+        if self.node is None:
+            self.logger = rclpy.logging.get_logger('database_server')
+        else:
+            self.logger = self.node.get_logger()
+
+        # Get current robot name from params (passed as parameter since ROS2 doesn't use global params)
+        # For now, we'll default to 'charon' - this should be passed as a parameter
+        self.robot_name = 'charon'  # TODO: Make this configurable
 
         if self.robot_name not in self.robot_configs.keys():
-            rospy.logerr(f"{self.robot_name} does not exist in robot_configs")
-            rospy.signal_shutdown("robot_name does not exist in robot_configs")
-            rospy.spin()
+            self.logger.error(f"{self.robot_name} does not exist in robot_configs")
+            raise ValueError("robot_name does not exist in robot_configs")
 
         self.topic_list = self.topic_configs[self.robot_configs[self.robot_name]["node-type"]]
 
@@ -42,68 +51,105 @@ def __init__(self, robot_configs, topic_configs):
 
         # create services for all the possible calls to the DB
         self.service_list = []
-        s = rospy.Service('~AddUpdateDB',
-                          mocha_core.srv.AddUpdateDB,
-                          self.add_update_db_service_cb)
-        self.service_list.append(s)
-        s = rospy.Service('~GetDataHeaderDB',
-                          mocha_core.srv.GetDataHeaderDB,
-                          self.get_data_hash_db_service_cb)
-        self.service_list.append(s)
-        s = rospy.Service('~SelectDB',
-                          mocha_core.srv.SelectDB,
-                          self.select_db_service_cb)
-        self.service_list.append(s)
+        if self.node is not None:
+            s = self.node.create_service(mocha_core.srv.AddUpdateDB,
+                                        'AddUpdateDB',
+                                        self.add_update_db_service_cb)
+            self.service_list.append(s)
+            s = self.node.create_service(mocha_core.srv.GetDataHeaderDB,
+                                        'GetDataHeaderDB',
+                                        self.get_data_hash_db_service_cb)
+            self.service_list.append(s)
+            s = self.node.create_service(mocha_core.srv.SelectDB,
+                                        'SelectDB',
+                                        self.select_db_service_cb)
+            self.service_list.append(s)
 
         self.msg_types = du.msg_types(self.robot_configs, self.topic_configs)
 
     def add_update_db_service_cb(self, req):
-        if not isinstance(req.topic_id, int) or req.topic_id is None:
-            rospy.logerr("Error: topic_id empty")
-            return
-        if len(req.msg_content) == 0:
-            rospy.logerr("Error: msg_content empty")
-            return
-        if req.topic_id > len(self.topic_list):
-            rospy.logerr("Error: topic_id not in topic_list")
-            return
-        topic = self.topic_list[req.topic_id]
-        priority = du.get_priority_number(topic["msg_priority"])
-        ts = req.timestamp
-        ts = rospy.Time(ts.secs, ts.nsecs)
-        dbm = database.DBMessage(self.robot_number,
-                                 req.topic_id,
-                                 dtype=self.msg_types[self.robot_number][req.topic_id]["dtype"],
-                                 priority=priority,
-                                 ts=ts,
-                                 data=req.msg_content)
-
-        header = self.dbl.add_modify_data(dbm)
-        return mocha_core.srv.AddUpdateDBResponse(header)
+        try:
+            if not isinstance(req.topic_id, int) or req.topic_id is None:
+                self.logger.error("topic_id empty")
+                return mocha_core.srv.AddUpdateDB.Response()
+            if len(req.msg_content) == 0:
+                self.logger.error("msg_content empty")
+                return mocha_core.srv.AddUpdateDB.Response()
+            if req.topic_id >= len(self.topic_list):  # Changed > to >= for proper bounds check
+                self.logger.error(f"topic_id {req.topic_id} not in topic_list (length: {len(self.topic_list)})")
+                return mocha_core.srv.AddUpdateDB.Response()
+            
+            topic = self.topic_list[req.topic_id]
+            priority = du.get_priority_number(topic["msg_priority"])
+            ts = req.timestamp
+            # ROS2 builtin_interfaces/Time uses 'sec' and 'nanosec' fields
+            ts = rclpy.time.Time(seconds=ts.sec, nanoseconds=ts.nanosec)
+            
+            # Convert array to bytes if needed (ROS2 service messages use array)
+            msg_data = req.msg_content
+            if hasattr(msg_data, 'tobytes'):
+                msg_data = msg_data.tobytes()
+            elif isinstance(msg_data, (list, tuple)):
+                msg_data = bytes(msg_data)
+            
+            dbm = database.DBMessage(self.robot_number,
+                                     req.topic_id,
+                                     dtype=self.msg_types[self.robot_number][req.topic_id]["dtype"],
+                                     priority=priority,
+                                     ts=ts,
+                                     data=msg_data)
+
+            header = self.dbl.add_modify_data(dbm)
+            response = mocha_core.srv.AddUpdateDB.Response()
+            response.new_header = header
+            return response
+        except Exception as e:
+            self.logger.error(f"Exception in add_update_db_service_cb: {e}")
+            return mocha_core.srv.AddUpdateDB.Response()
 
     def get_data_hash_db_service_cb(self, req):
-        if req.msg_header is None or len(req.msg_header) == 0:
-            rospy.logerr("Error: msg_header empty")
-            return
-        dbm = self.dbl.find_header(req.msg_header)
-        answ = mocha_core.srv.GetDataHeaderDBResponse(dbm.robot_id,
-                                                                dbm.topic_id,
-                                                                dbm.ts,
-                                                                dbm.data)
-        return answ
+        try:
+            if req.msg_header is None or len(req.msg_header) == 0:
+                self.logger.error("msg_header empty")
+                return mocha_core.srv.GetDataHeaderDB.Response()
+            
+            # Convert array to bytes if needed (ROS2 service messages use array)
+            header_data = req.msg_header
+            if hasattr(header_data, 'tobytes'):
+                header_data = header_data.tobytes()
+            elif isinstance(header_data, (list, tuple)):
+                header_data = bytes(header_data)
+            
+            dbm = self.dbl.find_header(header_data)
+            
+            response = mocha_core.srv.GetDataHeaderDB.Response()
+            response.robot_id = dbm.robot_id
+            response.topic_id = dbm.topic_id
+            response.timestamp = dbm.ts
+            response.msg_content = dbm.data
+            return response
+        except Exception as e:
+            self.logger.error(f"Exception in get_data_hash_db_service_cb: {e}")
+            return mocha_core.srv.GetDataHeaderDB.Response()
 
     def select_db_service_cb(self, req):
-        # TODO Implement filtering
-        if req.robot_id is None:
-            rospy.logerr("Error: robot_id none")
-            return
-        if req.topic_id is None:
-            rospy.logerr("Error: topic_id none")
-            return
-        list_headers = self.dbl.get_header_list(req.robot_id)
-        answ = mocha_core.srv.SelectDBResponse(du.serialize_headers(list_headers))
-        return answ
+        try:
+            # TODO Implement filtering
+            response = mocha_core.srv.SelectDB.Response()
+            
+            # 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)
+            return response
+        except Exception as e:
+            self.logger.error(f"Exception in select_db_service_cb: {e}")
+            response = mocha_core.srv.SelectDB.Response()
+            response.headers = b''
+            return response
 
     def shutdown(self):
-        for s in self.service_list:
-            s.shutdown()
+        # In ROS2, services are automatically cleaned up when the node is destroyed
+        pass

mocha_core/mocha_core/database_utils.py

@@ -133,19 +133,41 @@ def unpack_data(header, packed_data):
 
 def serialize_ros_msg(msg):
     # TODO check that we are not entering garbage
-    sio_h = io.BytesIO()
-    msg.serialize(sio_h)
-    serialized = sio_h.getvalue()
+    # In ROS2, we use CDR serialization
+    from rclpy.serialization import serialize_message
+    serialized = serialize_message(msg)
     compressed = lz4.frame.compress(serialized)
+    
+    # Debug: Test round-trip compression
+    try:
+        test_decompress = lz4.frame.decompress(compressed)
+        if test_decompress != serialized:
+            raise Exception("LZ4 round-trip test failed")
+    except Exception as e:
+        raise Exception(f"LZ4 compression test failed: {e}")
+    
     return compressed
 
 
 def parse_answer(api_answer, msg_types):
     constructor = msg_types[api_answer.robot_id][api_answer.topic_id]['obj']
-    msg = constructor()
     # api_answer.msg_content has the compressed message
-    decompressed = lz4.frame.decompress(api_answer.msg_content)
-    msg.deserialize(decompressed)
+    
+    # Debug: Check what we're trying to decompress
+    print(f"DEBUG: Trying to decompress {len(api_answer.msg_content)} bytes")
+    print(f"DEBUG: First 20 bytes: {api_answer.msg_content[:20]}")
+    
+    try:
+        decompressed = lz4.frame.decompress(api_answer.msg_content)
+    except Exception as e:
+        print(f"DEBUG: LZ4 decompression failed: {e}")
+        print(f"DEBUG: msg_content type: {type(api_answer.msg_content)}")
+        print(f"DEBUG: msg_content length: {len(api_answer.msg_content)}")
+        raise
+    
+    # In ROS2, we use CDR deserialization
+    from rclpy.serialization import deserialize_message
+    msg = deserialize_message(decompressed, constructor)
     robot_id = api_answer.robot_id
     topic_id = api_answer.topic_id
     ts = api_answer.timestamp