From 52c261300852b31e4580935afed066b780b8accc Mon Sep 17 00:00:00 2001 From: Nick Hawes Date: Fri, 5 Aug 2022 14:07:50 +0100 Subject: [PATCH 01/25] Revert "Revert "Fix Python 3 bugs in mongodb_store"" --- mongodb_store/CMakeLists.txt | 15 ++++++++++++--- mongodb_store/src/mongodb_store/message_store.py | 6 +++--- mongodb_store/src/mongodb_store/util.py | 16 ++++++---------- mongodb_store/tests/test_messagestore.py | 15 ++++++--------- 4 files changed, 27 insertions(+), 25 deletions(-) diff --git a/mongodb_store/CMakeLists.txt b/mongodb_store/CMakeLists.txt index febdaf3..2d4f454 100644 --- a/mongodb_store/CMakeLists.txt +++ b/mongodb_store/CMakeLists.txt @@ -133,9 +133,18 @@ target_link_libraries(example_multi_event_log ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination -install(DIRECTORY scripts/ - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} - USE_SOURCE_PERMISSIONS) +catkin_install_python(PROGRAMS + scripts/config_manager.py + scripts/example_message_store_client.py + scripts/example_multi_event_log.py + scripts/message_store_node.py + scripts/mongo_bridge.py + scripts/mongodb_play.py + scripts/mongodb_server.py + scripts/replicator_client.py + scripts/replicator_node.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) # Mark other files for installation (e.g. launch and bag files, etc.) install( diff --git a/mongodb_store/src/mongodb_store/message_store.py b/mongodb_store/src/mongodb_store/message_store.py index 1f5c0f2..c752bfd 100644 --- a/mongodb_store/src/mongodb_store/message_store.py +++ b/mongodb_store/src/mongodb_store/message_store.py @@ -261,8 +261,8 @@ def query(self, type, message_query = {}, meta_query = {}, single = False, sort_ messages = [] metas = [] else: - messages = map(dc_util.deserialise_message, response.messages) - metas = map(dc_util.string_pair_list_to_dictionary, response.metas) + messages = list(map(dc_util.deserialise_message, response.messages)) + metas = list(map(dc_util.string_pair_list_to_dictionary, response.metas)) if single: if len(messages) > 0: @@ -270,4 +270,4 @@ def query(self, type, message_query = {}, meta_query = {}, single = False, sort_ else: return [None, None] else: - return zip(messages,metas) + return list(zip(messages,metas)) diff --git a/mongodb_store/src/mongodb_store/util.py b/mongodb_store/src/mongodb_store/util.py index 369d62c..7d0f571 100644 --- a/mongodb_store/src/mongodb_store/util.py +++ b/mongodb_store/src/mongodb_store/util.py @@ -10,10 +10,10 @@ import platform if float(platform.python_version()[0:2]) >= 3.0: _PY3 = True - import io as StringIO + from io import BytesIO as Buffer else: _PY3 = False - import StringIO + from StringIO import StringIO as Buffer from mongodb_store_msgs.msg import SerialisedMessage from mongodb_store_msgs.srv import MongoQueryMsgRequest @@ -201,16 +201,13 @@ def sanitize_value(attr, v, type): else: # ensure unicode try: - if _PY3: - v = str(v, "utf-8") - else: + if not _PY3: # All strings are unicode in Python 3 v = unicode(v, "utf-8") except UnicodeDecodeError as e: # at this point we can deal with the encoding, so treat it as binary v = Binary(v) # no need to carry on with the other type checks below return v - if isinstance(v, rospy.Message): return msg_to_document(v) elif isinstance(v, genpy.rostime.Time): @@ -332,8 +329,7 @@ def fill_message(message, document): lst.append(msg) setattr(message, slot, lst) else: - if ( (not _PY3 and isinstance(value, unicode)) or - (_PY3 and isinstance(value, str)) ): + if not _PY3 and isinstance(value, unicode): # All strings are unicode in Python 3 setattr(message, slot, value.encode('utf-8')) else: setattr(message, slot, value) @@ -518,9 +514,9 @@ def serialise_message(message): :Args: | message (ROS message): The message to serialise :Returns: - | mongodb_store_msgs.msg.SerialisedMessage: A serialies copy of message + | mongodb_store_msgs.msg.SerialisedMessage: A serialised copy of message """ - buf=StringIO.StringIO() + buf = Buffer() message.serialize(buf) serialised_msg = SerialisedMessage() serialised_msg.msg = buf.getvalue() diff --git a/mongodb_store/tests/test_messagestore.py b/mongodb_store/tests/test_messagestore.py index 1aa0887..a406d40 100755 --- a/mongodb_store/tests/test_messagestore.py +++ b/mongodb_store/tests/test_messagestore.py @@ -63,19 +63,16 @@ def test_add_message(self): # get documents with limit result_limited = msg_store.query(Pose._type, message_query={'orientation.z': {'$gt': 10} }, sort_query=[("$natural", 1)], limit=10) self.assertEqual(len(result_limited), 10) - self.assertListEqual([int(doc[0].orientation.x) for doc in result_limited], range(10)) + self.assertListEqual([int(doc[0].orientation.x) for doc in result_limited], list(range(10))) - #get documents without "orientation" field - result_no_id = msg_store.query(Pose._type, message_query={}, projection_query={"orientation": 0}) + #get documents without "orientation" field + result_no_id = msg_store.query(Pose._type, message_query={}, projection_query={"orientation": 0}) for doc in result_no_id: - self.assertEqual(int(doc[0].orientation.z),0 ) - - - + self.assertEqual(int(doc[0].orientation.z), 0) # must remove the item or unittest only really valid once - print meta["_id"] - print str(meta["_id"]) + print(meta["_id"]) + print(str(meta["_id"])) deleted = msg_store.delete(str(meta["_id"])) self.assertTrue(deleted) From a12c2790d962c94edc8f38dffedb95406fb3c2af Mon Sep 17 00:00:00 2001 From: Gal Gorjup Date: Wed, 7 Sep 2022 13:21:30 +0000 Subject: [PATCH 02/25] Ensure Python 3 compatibility in mongodb log --- mongodb_log/CMakeLists.txt | 2 +- mongodb_log/scripts/mongodb_log.py | 17 ++++++++++------- mongodb_log/test/test_publisher.py | 2 +- 3 files changed, 12 insertions(+), 9 deletions(-) diff --git a/mongodb_log/CMakeLists.txt b/mongodb_log/CMakeLists.txt index 239481e..ff065a6 100644 --- a/mongodb_log/CMakeLists.txt +++ b/mongodb_log/CMakeLists.txt @@ -138,7 +138,7 @@ target_link_libraries(mongodb_log_cimg ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination -install(PROGRAMS +catkin_install_python(PROGRAMS scripts/mongodb_log.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) diff --git a/mongodb_log/scripts/mongodb_log.py b/mongodb_log/scripts/mongodb_log.py index b12b7b3..bd6b70c 100755 --- a/mongodb_log/scripts/mongodb_log.py +++ b/mongodb_log/scripts/mongodb_log.py @@ -46,7 +46,10 @@ import subprocess from threading import Thread, Timer -from Queue import Empty +try: + from queue import Empty +except ImportError: + from Queue import Empty from optparse import OptionParser from tempfile import mktemp from datetime import datetime, timedelta @@ -271,12 +274,12 @@ def dequeue(self): mongodb_store.util.store_message(self.collection, msg, meta) - except InvalidDocument, e: + except InvalidDocument as e: print("InvalidDocument " + current_process().name + "@" + topic +": \n") - print e - except InvalidStringData, e: + print(e) + except InvalidStringData as e: print("InvalidStringData " + current_process().name + "@" + topic +": \n") - print e + print(e) else: #print("Quit W2: %s" % self.name) @@ -447,7 +450,7 @@ def subscribe_topics(self, topics): self.workers.append(w) self.collnames |= set([collname]) self.topics |= set([topic]) - except Exception, e: + except Exception as e: print('Failed to subscribe to %s due to %s' % (topic, e)) missing_topics.add(topic) @@ -457,7 +460,7 @@ def subscribe_topics(self, topics): def create_worker(self, idnum, topic, collname): try: msg_class, real_topic, msg_eval = rostopic.get_topic_class(topic, blocking=False) - except Exception, e: + except Exception as e: print('Topic %s not announced, cannot get type: %s' % (topic, e)) raise diff --git a/mongodb_log/test/test_publisher.py b/mongodb_log/test/test_publisher.py index 2c368e9..e4fbef6 100755 --- a/mongodb_log/test/test_publisher.py +++ b/mongodb_log/test/test_publisher.py @@ -45,6 +45,6 @@ def publish(self): for target in to_publish: msg_store = MessageStoreProxy(database='roslog', collection=target[0]) - print len(msg_store.query(Int64._type)) == target[3] + print(len(msg_store.query(Int64._type)) == target[3]) From 99b979c58d720a8b17520debd1493aa261570db7 Mon Sep 17 00:00:00 2001 From: Gal Gorjup Date: Wed, 14 Sep 2022 08:41:45 +0000 Subject: [PATCH 03/25] Avoid deadlock on server shutdown --- mongodb_store/scripts/config_manager.py | 13 ++++++++++--- mongodb_store/scripts/mongodb_server.py | 2 +- mongodb_store/tests/config_manager.test | 4 ++-- 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/mongodb_store/scripts/config_manager.py b/mongodb_store/scripts/config_manager.py index b77dc69..d0f1e39 100755 --- a/mongodb_store/scripts/config_manager.py +++ b/mongodb_store/scripts/config_manager.py @@ -82,17 +82,17 @@ class ConfigManager(object): def __init__(self): rospy.init_node("config_manager") - use_daemon = rospy.get_param('mongodb_use_daemon', False) + self.use_daemon = rospy.get_param('mongodb_use_daemon', False) connection_string = rospy.get_param('/mongodb_connection_string', '') use_connection_string = len(connection_string) > 0 if use_connection_string: - use_daemon = True + self.use_daemon = True rospy.loginfo('Using connection string: %s', connection_string) db_host = rospy.get_param('mongodb_host') db_port = rospy.get_param('mongodb_port') - if use_daemon: + if self.use_daemon: if use_connection_string: is_daemon_alive = mongodb_store.util.check_connection_to_mongod(None, None, connection_string=connection_string) else: @@ -243,6 +243,13 @@ def _list_params(self): def _on_node_shutdown(self): + # Prevent concurrent calls to mongod process during shutdown to avoid deadlock + if not self.use_daemon: + try: + mongodb_store.util.wait_for_mongo(timeout=2) # The server is marked "not ready" just before shutdown + except rospy.service.ServiceException: # If the server node exits during the service call + pass + # Close Mongo Client try: # PyMongo 2.9 or later self._mongo_client.close() diff --git a/mongodb_store/scripts/mongodb_server.py b/mongodb_store/scripts/mongodb_server.py index c3c1bd8..a7ec139 100755 --- a/mongodb_store/scripts/mongodb_server.py +++ b/mongodb_store/scripts/mongodb_server.py @@ -160,10 +160,10 @@ def block_mongo_kill(): rospy.logerr("Mongo process error! Exit code="+str(self._mongo_process.returncode)) self._gone_down = True - self._ready=False def _on_node_shutdown(self): rospy.loginfo("Shutting down datacentre") + self._ready=False if self._gone_down: rospy.logwarn("It looks like Mongo already died. Watch out as the DB might need recovery time at next run.") return diff --git a/mongodb_store/tests/config_manager.test b/mongodb_store/tests/config_manager.test index 827541e..b710864 100644 --- a/mongodb_store/tests/config_manager.test +++ b/mongodb_store/tests/config_manager.test @@ -1,8 +1,8 @@ - + -j + From 500ba7edf0f31d9ad8cbe47cf866f26444705e9b Mon Sep 17 00:00:00 2001 From: Gal Gorjup Date: Fri, 16 Sep 2022 08:57:27 +0000 Subject: [PATCH 04/25] Skip closing client on shutdown to avoid deadlock --- mongodb_store/scripts/config_manager.py | 23 +++-------------------- 1 file changed, 3 insertions(+), 20 deletions(-) diff --git a/mongodb_store/scripts/config_manager.py b/mongodb_store/scripts/config_manager.py index d0f1e39..11e8d57 100755 --- a/mongodb_store/scripts/config_manager.py +++ b/mongodb_store/scripts/config_manager.py @@ -82,17 +82,17 @@ class ConfigManager(object): def __init__(self): rospy.init_node("config_manager") - self.use_daemon = rospy.get_param('mongodb_use_daemon', False) + use_daemon = rospy.get_param('mongodb_use_daemon', False) connection_string = rospy.get_param('/mongodb_connection_string', '') use_connection_string = len(connection_string) > 0 if use_connection_string: - self.use_daemon = True + use_daemon = True rospy.loginfo('Using connection string: %s', connection_string) db_host = rospy.get_param('mongodb_host') db_port = rospy.get_param('mongodb_port') - if self.use_daemon: + if use_daemon: if use_connection_string: is_daemon_alive = mongodb_store.util.check_connection_to_mongod(None, None, connection_string=connection_string) else: @@ -108,8 +108,6 @@ def __init__(self): else: self._mongo_client=MongoClient(db_host, db_port) - rospy.on_shutdown(self._on_node_shutdown) - self._database=self._mongo_client.config self._database.add_son_manipulator(MongoTransformer()) @@ -241,21 +239,6 @@ def _list_params(self): print(name, " "*(30-len(name)),val," "*(30-len(str(val))),filename) print() - - def _on_node_shutdown(self): - # Prevent concurrent calls to mongod process during shutdown to avoid deadlock - if not self.use_daemon: - try: - mongodb_store.util.wait_for_mongo(timeout=2) # The server is marked "not ready" just before shutdown - except rospy.service.ServiceException: # If the server node exits during the service call - pass - # Close Mongo Client - try: - # PyMongo 2.9 or later - self._mongo_client.close() - except Exception as e: - self._mongo_client.disconnect() - # Could just use the ros parameter server to get the params # but one day might not back onto the parameter server... def _getparam_srv_cb(self,req): From ee5f992034e8a31fc862b320e30e56284fff77d8 Mon Sep 17 00:00:00 2001 From: Gal Gorjup Date: Tue, 20 Sep 2022 11:34:29 +0200 Subject: [PATCH 05/25] Update changelogs --- mongodb_log/CHANGELOG.rst | 6 ++++++ mongodb_store/CHANGELOG.rst | 17 +++++++++++++++++ mongodb_store_msgs/CHANGELOG.rst | 3 +++ 3 files changed, 26 insertions(+) diff --git a/mongodb_log/CHANGELOG.rst b/mongodb_log/CHANGELOG.rst index 2e3618e..5000462 100644 --- a/mongodb_log/CHANGELOG.rst +++ b/mongodb_log/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package mongodb_log ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Ensure Python 3 compatibility in mongodb log (`#277 `_) +* update package.xml to format=3 (`#269 `_) +* Contributors: Gal Gorjup, Kei Okada, Nick Hawes + 0.5.2 (2019-11-11) ------------------ * back to system mongo diff --git a/mongodb_store/CHANGELOG.rst b/mongodb_store/CHANGELOG.rst index 4ed0bb1..9fe9500 100644 --- a/mongodb_store/CHANGELOG.rst +++ b/mongodb_store/CHANGELOG.rst @@ -2,6 +2,23 @@ Changelog for package mongodb_store ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- +* Avoid deadlock on server shutdown (`#279 `_) +* Fix Python 3 bugs in mongodb_store (`#272 `_, `#274 `_, `#275 `_) +* handling of host binding (`#270 `_) +* update package.xml to format=3 (`#269 `_) +* fix connection_string arg default value (`#266 `_) +* fixed bug in where the replicator node did not recognize the db_host (`#261 `_) +* Added .launch to the roslaunch command that was written in the readme file (`#262 `_) +* fixed a formatting issue +* fixed bug in where the replicator node did not recognize the db_host +* remembering namespace in rosparam +* Provide options to prevent unnecessary nodes launching +* added ability for message store to use a full connection string +* Removed --smallfiles arg no longer supported by MongoDB (`#257 `_) +* Contributors: Adrian Dole, Gal Gorjup, Kei Okada, Marc Hanheide, Nick Hawes, Shingo Kitagawa, Vittoria Santoro + 0.5.2 (2019-11-11) ------------------ * added python-future to package.xml, which got lost in previous commit for some reasons ... diff --git a/mongodb_store_msgs/CHANGELOG.rst b/mongodb_store_msgs/CHANGELOG.rst index f737357..f4378b3 100644 --- a/mongodb_store_msgs/CHANGELOG.rst +++ b/mongodb_store_msgs/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package mongodb_store_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Forthcoming +----------- + 0.5.2 (2019-11-11) ------------------ From 0ab1a5d65bd84b7b7eacc2b12aba8222345c6d6d Mon Sep 17 00:00:00 2001 From: Gal Gorjup Date: Tue, 20 Sep 2022 11:59:04 +0200 Subject: [PATCH 06/25] 0.6.0 --- mongodb_log/CHANGELOG.rst | 4 ++-- mongodb_log/package.xml | 2 +- mongodb_store/CHANGELOG.rst | 4 ++-- mongodb_store/package.xml | 2 +- mongodb_store_msgs/CHANGELOG.rst | 4 ++-- mongodb_store_msgs/package.xml | 2 +- 6 files changed, 9 insertions(+), 9 deletions(-) diff --git a/mongodb_log/CHANGELOG.rst b/mongodb_log/CHANGELOG.rst index 5000462..de0b687 100644 --- a/mongodb_log/CHANGELOG.rst +++ b/mongodb_log/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mongodb_log ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.6.0 (2022-09-20) +------------------ * Ensure Python 3 compatibility in mongodb log (`#277 `_) * update package.xml to format=3 (`#269 `_) * Contributors: Gal Gorjup, Kei Okada, Nick Hawes diff --git a/mongodb_log/package.xml b/mongodb_log/package.xml index d1fe6e2..2777bf9 100644 --- a/mongodb_log/package.xml +++ b/mongodb_log/package.xml @@ -1,7 +1,7 @@ mongodb_log - 0.5.2 + 0.6.0 The mongodb_log package Tim Niemueller diff --git a/mongodb_store/CHANGELOG.rst b/mongodb_store/CHANGELOG.rst index 9fe9500..46cb58b 100644 --- a/mongodb_store/CHANGELOG.rst +++ b/mongodb_store/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mongodb_store ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.6.0 (2022-09-20) +------------------ * Avoid deadlock on server shutdown (`#279 `_) * Fix Python 3 bugs in mongodb_store (`#272 `_, `#274 `_, `#275 `_) * handling of host binding (`#270 `_) diff --git a/mongodb_store/package.xml b/mongodb_store/package.xml index 495aab8..5e38a32 100644 --- a/mongodb_store/package.xml +++ b/mongodb_store/package.xml @@ -1,7 +1,7 @@ mongodb_store - 0.5.2 + 0.6.0 A package to support MongoDB-based storage and analysis for data from a ROS system, eg. saved messages, configurations etc Nick Hawes diff --git a/mongodb_store_msgs/CHANGELOG.rst b/mongodb_store_msgs/CHANGELOG.rst index f4378b3..fbeae08 100644 --- a/mongodb_store_msgs/CHANGELOG.rst +++ b/mongodb_store_msgs/CHANGELOG.rst @@ -2,8 +2,8 @@ Changelog for package mongodb_store_msgs ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -Forthcoming ------------ +0.6.0 (2022-09-20) +------------------ 0.5.2 (2019-11-11) ------------------ diff --git a/mongodb_store_msgs/package.xml b/mongodb_store_msgs/package.xml index 58f93c1..0d9b72a 100644 --- a/mongodb_store_msgs/package.xml +++ b/mongodb_store_msgs/package.xml @@ -1,7 +1,7 @@ mongodb_store_msgs - 0.5.2 + 0.6.0 The mongodb_store_msgs package Nick Hawes From 22b926106b54e7e8501e6404f90e750dc30e3dc2 Mon Sep 17 00:00:00 2001 From: Shingo Kitagawa Date: Wed, 8 Feb 2023 17:37:40 +0900 Subject: [PATCH 07/25] add host args --- mongodb_store/launch/mongodb_store.launch | 2 ++ mongodb_store/launch/mongodb_store_inc.launch | 5 +++-- 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/mongodb_store/launch/mongodb_store.launch b/mongodb_store/launch/mongodb_store.launch index 8ff0bfd..f55c7c5 100644 --- a/mongodb_store/launch/mongodb_store.launch +++ b/mongodb_store/launch/mongodb_store.launch @@ -4,6 +4,7 @@ + @@ -30,6 +31,7 @@ + diff --git a/mongodb_store/launch/mongodb_store_inc.launch b/mongodb_store/launch/mongodb_store_inc.launch index 1dce50f..e2dd1d5 100644 --- a/mongodb_store/launch/mongodb_store_inc.launch +++ b/mongodb_store/launch/mongodb_store_inc.launch @@ -3,6 +3,7 @@ + @@ -29,7 +30,7 @@ - + @@ -42,7 +43,7 @@ - + From 8734f19e5576d3bea211d81011aee6ea234c347a Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Mon, 14 Apr 2025 13:51:45 +0100 Subject: [PATCH 08/25] mongodb_store_msgs updated to ros2 package format --- mongodb_store_msgs/CMakeLists.txt | 49 +++++++++----------- mongodb_store_msgs/action/MoveEntries.action | 2 +- mongodb_store_msgs/package.xml | 25 +++++----- 3 files changed, 35 insertions(+), 41 deletions(-) diff --git a/mongodb_store_msgs/CMakeLists.txt b/mongodb_store_msgs/CMakeLists.txt index ed1c751..0900219 100644 --- a/mongodb_store_msgs/CMakeLists.txt +++ b/mongodb_store_msgs/CMakeLists.txt @@ -1,35 +1,28 @@ -cmake_minimum_required(VERSION 2.8.3) +cmake_minimum_required(VERSION 3.10) project(mongodb_store_msgs) -find_package(catkin REQUIRED COMPONENTS message_generation actionlib actionlib_msgs) - -add_message_files( - FILES - StringList.msg - StringPair.msg - StringPairList.msg - SerialisedMessage.msg - Insert.msg -) - -add_service_files( - FILES - MongoInsertMsg.srv - MongoUpdateMsg.srv - MongoQueryMsg.srv - MongoQuerywithProjectionMsg.srv - MongoDeleteMsg.srv +if(POLICY CMP0148) +# don't show warnings about this policy +cmake_policy(SET CMP0148 OLD) +endif() + +find_package(ament_cmake) +find_package(std_msgs) +find_package(action_msgs) +find_package(builtin_interfaces) +find_package(rosidl_default_generators) + +file(GLOB msg_files RELATIVE "${CMAKE_CURRENT_LIST_DIR}" + "${CMAKE_CURRENT_LIST_DIR}/msg/*.msg" + "${CMAKE_CURRENT_LIST_DIR}/srv/*.srv" + "${CMAKE_CURRENT_LIST_DIR}/action/*.action" ) -add_action_files( - FILES - MoveEntries.action +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + DEPENDENCIES std_msgs action_msgs builtin_interfaces ) +ament_export_dependencies(rosidl_default_runtime) -generate_messages(DEPENDENCIES actionlib_msgs) - -catkin_package( - CATKIN_DEPENDS message_generation -) - +ament_package() \ No newline at end of file diff --git a/mongodb_store_msgs/action/MoveEntries.action b/mongodb_store_msgs/action/MoveEntries.action index 4fc24a3..72807ef 100644 --- a/mongodb_store_msgs/action/MoveEntries.action +++ b/mongodb_store_msgs/action/MoveEntries.action @@ -3,7 +3,7 @@ string database # the collections to move entries from StringList collections # only entries before rospy.get_rostime() - move_before are moved. if 0, all are moved -duration move_before +builtin_interfaces/Duration move_before # delete moved entries after replication bool delete_after_move # query to move entries by diff --git a/mongodb_store_msgs/package.xml b/mongodb_store_msgs/package.xml index 0d9b72a..5489e47 100644 --- a/mongodb_store_msgs/package.xml +++ b/mongodb_store_msgs/package.xml @@ -1,5 +1,5 @@ - + mongodb_store_msgs 0.6.0 The mongodb_store_msgs package @@ -13,18 +13,19 @@ MIT Nick Hawes - - catkin - catkin - message_generation - message_runtime - message_generation + ament_cmake -actionlib_msgs - actionlib + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + _msgs + action_msgs + builtin_interfaces + + + ament_cmake + -actionlib_msgs - actionlib - From 54c9f9a8698d1a73e66151ddf2e0f1a44ccb30c3 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Mon, 14 Apr 2025 13:52:16 +0100 Subject: [PATCH 09/25] move srvs from mongodb store to store msgs --- {mongodb_store => mongodb_store_msgs}/srv/GetParam.srv | 0 {mongodb_store => mongodb_store_msgs}/srv/MongoFind.srv | 0 {mongodb_store => mongodb_store_msgs}/srv/MongoInsert.srv | 0 {mongodb_store => mongodb_store_msgs}/srv/MongoUpdate.srv | 0 {mongodb_store => mongodb_store_msgs}/srv/SetParam.srv | 0 5 files changed, 0 insertions(+), 0 deletions(-) rename {mongodb_store => mongodb_store_msgs}/srv/GetParam.srv (100%) rename {mongodb_store => mongodb_store_msgs}/srv/MongoFind.srv (100%) rename {mongodb_store => mongodb_store_msgs}/srv/MongoInsert.srv (100%) rename {mongodb_store => mongodb_store_msgs}/srv/MongoUpdate.srv (100%) rename {mongodb_store => mongodb_store_msgs}/srv/SetParam.srv (100%) diff --git a/mongodb_store/srv/GetParam.srv b/mongodb_store_msgs/srv/GetParam.srv similarity index 100% rename from mongodb_store/srv/GetParam.srv rename to mongodb_store_msgs/srv/GetParam.srv diff --git a/mongodb_store/srv/MongoFind.srv b/mongodb_store_msgs/srv/MongoFind.srv similarity index 100% rename from mongodb_store/srv/MongoFind.srv rename to mongodb_store_msgs/srv/MongoFind.srv diff --git a/mongodb_store/srv/MongoInsert.srv b/mongodb_store_msgs/srv/MongoInsert.srv similarity index 100% rename from mongodb_store/srv/MongoInsert.srv rename to mongodb_store_msgs/srv/MongoInsert.srv diff --git a/mongodb_store/srv/MongoUpdate.srv b/mongodb_store_msgs/srv/MongoUpdate.srv similarity index 100% rename from mongodb_store/srv/MongoUpdate.srv rename to mongodb_store_msgs/srv/MongoUpdate.srv diff --git a/mongodb_store/srv/SetParam.srv b/mongodb_store_msgs/srv/SetParam.srv similarity index 100% rename from mongodb_store/srv/SetParam.srv rename to mongodb_store_msgs/srv/SetParam.srv From afd86e1b105327c5508ec468ec005e1b38733f2e Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Mon, 14 Apr 2025 14:31:18 +0100 Subject: [PATCH 10/25] initial conversion of mongodb_store package files and structure to ros2 --- mongodb_store/CMakeLists.txt | 227 ++++++++---------- ...nc.launch => mongodb_store_inc_launch.xml} | 48 ++-- ..._store.launch => mongodb_store_launch.xml} | 32 +-- .../{src => }/mongodb_store/__init__.py | 0 .../{src => }/mongodb_store/message_store.py | 0 .../mongodb_store/scripts/__init__.py | 0 .../scripts/config_manager.py | 0 .../scripts/example_message_store_client.py | 0 .../scripts/example_multi_event_log.py | 0 .../scripts/message_store_node.py | 0 .../scripts/mongo_bridge.py | 0 .../scripts/mongodb_play.py | 0 .../scripts/mongodb_server.py | 0 .../scripts/replicator_client.py | 0 .../scripts/replicator_node.py | 13 +- mongodb_store/{src => }/mongodb_store/util.py | 0 mongodb_store/package.xml | 62 +++-- mongodb_store/resource/mongodb_store | 0 mongodb_store/setup.cfg | 4 + mongodb_store/setup.py | 67 +++++- 20 files changed, 237 insertions(+), 216 deletions(-) rename mongodb_store/launch/{mongodb_store_inc.launch => mongodb_store_inc_launch.xml} (51%) rename mongodb_store/launch/{mongodb_store.launch => mongodb_store_launch.xml} (58%) rename mongodb_store/{src => }/mongodb_store/__init__.py (100%) rename mongodb_store/{src => }/mongodb_store/message_store.py (100%) create mode 100644 mongodb_store/mongodb_store/scripts/__init__.py rename mongodb_store/{ => mongodb_store}/scripts/config_manager.py (100%) rename mongodb_store/{ => mongodb_store}/scripts/example_message_store_client.py (100%) rename mongodb_store/{ => mongodb_store}/scripts/example_multi_event_log.py (100%) rename mongodb_store/{ => mongodb_store}/scripts/message_store_node.py (100%) rename mongodb_store/{ => mongodb_store}/scripts/mongo_bridge.py (100%) rename mongodb_store/{ => mongodb_store}/scripts/mongodb_play.py (100%) rename mongodb_store/{ => mongodb_store}/scripts/mongodb_server.py (100%) rename mongodb_store/{ => mongodb_store}/scripts/replicator_client.py (100%) rename mongodb_store/{ => mongodb_store}/scripts/replicator_node.py (98%) rename mongodb_store/{src => }/mongodb_store/util.py (100%) create mode 100644 mongodb_store/resource/mongodb_store create mode 100644 mongodb_store/setup.cfg diff --git a/mongodb_store/CMakeLists.txt b/mongodb_store/CMakeLists.txt index 2d4f454..18f28d0 100644 --- a/mongodb_store/CMakeLists.txt +++ b/mongodb_store/CMakeLists.txt @@ -1,58 +1,19 @@ cmake_minimum_required(VERSION 2.8.3) project(mongodb_store) -# for ROS indigo compile without c++11 support -if(DEFINED ENV{ROS_DISTRO}) - if(NOT $ENV{ROS_DISTRO} STREQUAL "indigo") - add_compile_options(-std=c++11) - message(STATUS "Building with C++11 support") - else() - message(STATUS "ROS Indigo: building without C++11 support") - endif() -else() - message(STATUS "Environmental variable ROS_DISTRO not defined, checking OS version") - file(STRINGS /etc/os-release RELEASE_CODENAME - REGEX "VERSION_CODENAME=") - if(NOT ${RELEASE_CODENAME} MATCHES "trusty") - add_compile_options(-std=c++11) - message(STATUS "OS distro is not trusty: building with C++11 support") - else() - message(STATUS "Ubuntu Trusty: building without C++11 support") - endif() -endif() - -find_package(catkin REQUIRED COMPONENTS roscpp message_generation rospy std_msgs std_srvs mongodb_store_msgs topic_tools) - -set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) -find_package(MongoClient REQUIRED) +find_package(ament_cmake) +find_package(std_msgs) +find_package(std_srvs) +find_package(mongodb_store_msgs) + +#set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) +#find_package(MongoClient REQUIRED) ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -catkin_python_setup() - -####################################### -## Declare ROS messages and services ## -####################################### - - -## Generate services in the 'srv' folder -add_service_files( - FILES - GetParam.srv - SetParam.srv - MongoFind.srv - MongoUpdate.srv - MongoInsert.srv -) - - -## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs -) +#catkin_python_setup() ################################### ## catkin specific configuration ## @@ -65,12 +26,12 @@ generate_messages( ## DEPENDS: system dependencies of this project that dependent projects also need -catkin_package( - INCLUDE_DIRS include - LIBRARIES message_store ${MongoClient_INCLUDE_DIR} - CATKIN_DEPENDS mongodb_store_msgs topic_tools - DEPENDS MongoClient -) +#catkin_package( +# INCLUDE_DIRS include +# LIBRARIES message_store ${MongoClient_INCLUDE_DIR} +# CATKIN_DEPENDS mongodb_store_msgs topic_tools +# DEPENDS MongoClient +#) ########### ## Build ## @@ -78,50 +39,50 @@ catkin_package( ## Specify additional locations of header files ## Your package locations should be listed before other locations -include_directories( - include - ${catkin_INCLUDE_DIRS} - ${MongoClient_INCLUDE_DIR} -) - -link_directories(${catkin_LINK_DIRS}) -link_directories(${MongoClient_LINK_DIRS}) - -add_library(message_store src/message_store.cpp ) - -add_executable(example_mongodb_store_cpp_client src/example_mongodb_store_cpp_client.cpp) -add_executable(example_multi_event_log src/example_multi_event_log.cpp) -# add_executable(pc_test src/point_cloud_test.cpp) - - -add_dependencies(example_mongodb_store_cpp_client mongodb_store_msgs_generate_messages_cpp ) -add_dependencies(message_store mongodb_store_msgs_generate_messages_cpp ) - -target_link_libraries(message_store - ${MongoClient_LIBRARIES} - ${catkin_LIBRARIES} -) +#include_directories( +# include +# ${catkin_INCLUDE_DIRS} +# ${MongoClient_INCLUDE_DIR} +#) +# +#link_directories(${catkin_LINK_DIRS}) +#link_directories(${MongoClient_LINK_DIRS}) +# +#add_library(message_store src/message_store.cpp ) +# +#add_executable(example_mongodb_store_cpp_client src/example_mongodb_store_cpp_client.cpp) +#add_executable(example_multi_event_log src/example_multi_event_log.cpp) +## add_executable(pc_test src/point_cloud_test.cpp) +# +# +#add_dependencies(example_mongodb_store_cpp_client mongodb_store_msgs_generate_messages_cpp ) +#add_dependencies(message_store mongodb_store_msgs_generate_messages_cpp ) +# +#target_link_libraries(message_store +# ${MongoClient_LIBRARIES} +# ${catkin_LIBRARIES} +#) # Specify libraries to link a library or executable target against -target_link_libraries(example_mongodb_store_cpp_client - message_store - ${MongoClient_LIBRARIES} - ${catkin_LIBRARIES} -) - -target_link_libraries(example_multi_event_log - message_store - ${MongoClient_LIBRARIES} - ${catkin_LIBRARIES} -) - -target_link_libraries(example_multi_event_log - message_store - ${MongoClient_LIBRARIES} - ${catkin_LIBRARIES} -) +#target_link_libraries(example_mongodb_store_cpp_client +# message_store +# ${MongoClient_LIBRARIES} +# ${catkin_LIBRARIES} +#) +# +#target_link_libraries(example_multi_event_log +# message_store +# ${MongoClient_LIBRARIES} +# ${catkin_LIBRARIES} +#) +# +#target_link_libraries(example_multi_event_log +# message_store +# ${MongoClient_LIBRARIES} +# ${catkin_LIBRARIES} +#) ############# @@ -133,18 +94,18 @@ target_link_libraries(example_multi_event_log ## Mark executable scripts (Python etc.) for installation ## in contrast to setup.py, you can choose the destination -catkin_install_python(PROGRAMS - scripts/config_manager.py - scripts/example_message_store_client.py - scripts/example_multi_event_log.py - scripts/message_store_node.py - scripts/mongo_bridge.py - scripts/mongodb_play.py - scripts/mongodb_server.py - scripts/replicator_client.py - scripts/replicator_node.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +#catkin_install_python(PROGRAMS +# scripts/config_manager.py +# scripts/example_message_store_client.py +# scripts/example_multi_event_log.py +# scripts/message_store_node.py +# scripts/mongo_bridge.py +# scripts/mongodb_play.py +# scripts/mongodb_server.py +# scripts/replicator_client.py +# scripts/replicator_node.py +# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +#) # Mark other files for installation (e.g. launch and bag files, etc.) install( @@ -153,39 +114,39 @@ install( ) # Mark cpp header files for installation -install(DIRECTORY include/mongodb_store/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) +#install(DIRECTORY include/mongodb_store/ +# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} +#) ## Mark executables and/or libraries for installation -install(TARGETS example_mongodb_store_cpp_client example_multi_event_log message_store #pc_test - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) +#install(TARGETS example_mongodb_store_cpp_client example_multi_event_log message_store #pc_test +# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} +# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +#) ############# ## Testing ## ############# -if (CATKIN_ENABLE_TESTING) - find_package(catkin REQUIRED COMPONENTS rostest) - - add_rostest(tests/message_store.test) - add_rostest(tests/config_manager.test) - add_rostest(tests/replication.test) - - add_executable(message_store_cpp_test tests/message_store_cpp_test.cpp) - - target_link_libraries(message_store_cpp_test - message_store - ${OPENSSL_LIBRARIES} - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - gtest - ) - - add_rostest(tests/message_store_cpp_client.test) - -endif() +#if (CATKIN_ENABLE_TESTING) +# find_package(catkin REQUIRED COMPONENTS rostest) +# +# add_rostest(tests/message_store.test) +# add_rostest(tests/config_manager.test) +# add_rostest(tests/replication.test) +# +# add_executable(message_store_cpp_test tests/message_store_cpp_test.cpp) +# +# target_link_libraries(message_store_cpp_test +# message_store +# ${OPENSSL_LIBRARIES} +# ${catkin_LIBRARIES} +# ${Boost_LIBRARIES} +# gtest +# ) +# +# add_rostest(tests/message_store_cpp_client.test) +# +#endif() diff --git a/mongodb_store/launch/mongodb_store_inc.launch b/mongodb_store/launch/mongodb_store_inc_launch.xml similarity index 51% rename from mongodb_store/launch/mongodb_store_inc.launch rename to mongodb_store/launch/mongodb_store_inc_launch.xml index e2dd1d5..736bca6 100644 --- a/mongodb_store/launch/mongodb_store_inc.launch +++ b/mongodb_store/launch/mongodb_store_inc_launch.xml @@ -11,7 +11,7 @@ - + @@ -24,49 +24,49 @@ - - + + - - - + + + - + - - + + - - - - - - - - + + + + + + + + - - + + - - - + + + - - + + diff --git a/mongodb_store/launch/mongodb_store.launch b/mongodb_store/launch/mongodb_store_launch.xml similarity index 58% rename from mongodb_store/launch/mongodb_store.launch rename to mongodb_store/launch/mongodb_store_launch.xml index f55c7c5..0cfb67b 100644 --- a/mongodb_store/launch/mongodb_store.launch +++ b/mongodb_store/launch/mongodb_store_launch.xml @@ -29,22 +29,22 @@ - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + diff --git a/mongodb_store/src/mongodb_store/__init__.py b/mongodb_store/mongodb_store/__init__.py similarity index 100% rename from mongodb_store/src/mongodb_store/__init__.py rename to mongodb_store/mongodb_store/__init__.py diff --git a/mongodb_store/src/mongodb_store/message_store.py b/mongodb_store/mongodb_store/message_store.py similarity index 100% rename from mongodb_store/src/mongodb_store/message_store.py rename to mongodb_store/mongodb_store/message_store.py diff --git a/mongodb_store/mongodb_store/scripts/__init__.py b/mongodb_store/mongodb_store/scripts/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/mongodb_store/scripts/config_manager.py b/mongodb_store/mongodb_store/scripts/config_manager.py similarity index 100% rename from mongodb_store/scripts/config_manager.py rename to mongodb_store/mongodb_store/scripts/config_manager.py diff --git a/mongodb_store/scripts/example_message_store_client.py b/mongodb_store/mongodb_store/scripts/example_message_store_client.py similarity index 100% rename from mongodb_store/scripts/example_message_store_client.py rename to mongodb_store/mongodb_store/scripts/example_message_store_client.py diff --git a/mongodb_store/scripts/example_multi_event_log.py b/mongodb_store/mongodb_store/scripts/example_multi_event_log.py similarity index 100% rename from mongodb_store/scripts/example_multi_event_log.py rename to mongodb_store/mongodb_store/scripts/example_multi_event_log.py diff --git a/mongodb_store/scripts/message_store_node.py b/mongodb_store/mongodb_store/scripts/message_store_node.py similarity index 100% rename from mongodb_store/scripts/message_store_node.py rename to mongodb_store/mongodb_store/scripts/message_store_node.py diff --git a/mongodb_store/scripts/mongo_bridge.py b/mongodb_store/mongodb_store/scripts/mongo_bridge.py similarity index 100% rename from mongodb_store/scripts/mongo_bridge.py rename to mongodb_store/mongodb_store/scripts/mongo_bridge.py diff --git a/mongodb_store/scripts/mongodb_play.py b/mongodb_store/mongodb_store/scripts/mongodb_play.py similarity index 100% rename from mongodb_store/scripts/mongodb_play.py rename to mongodb_store/mongodb_store/scripts/mongodb_play.py diff --git a/mongodb_store/scripts/mongodb_server.py b/mongodb_store/mongodb_store/scripts/mongodb_server.py similarity index 100% rename from mongodb_store/scripts/mongodb_server.py rename to mongodb_store/mongodb_store/scripts/mongodb_server.py diff --git a/mongodb_store/scripts/replicator_client.py b/mongodb_store/mongodb_store/scripts/replicator_client.py similarity index 100% rename from mongodb_store/scripts/replicator_client.py rename to mongodb_store/mongodb_store/scripts/replicator_client.py diff --git a/mongodb_store/scripts/replicator_node.py b/mongodb_store/mongodb_store/scripts/replicator_node.py similarity index 98% rename from mongodb_store/scripts/replicator_node.py rename to mongodb_store/mongodb_store/scripts/replicator_node.py index 251189d..117c298 100755 --- a/mongodb_store/scripts/replicator_node.py +++ b/mongodb_store/mongodb_store/scripts/replicator_node.py @@ -356,7 +356,14 @@ def do_cancel(self): self.restore_process.shutdown() self.restore_process = None +def main(): + rclpy.init() + node = rclpy.node.Node("mongodb_replicator") + store = Replicator(node) + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + if __name__ == '__main__': - rospy.init_node("mongodb_replicator") - store = Replicator() - rospy.spin() + main() \ No newline at end of file diff --git a/mongodb_store/src/mongodb_store/util.py b/mongodb_store/mongodb_store/util.py similarity index 100% rename from mongodb_store/src/mongodb_store/util.py rename to mongodb_store/mongodb_store/util.py diff --git a/mongodb_store/package.xml b/mongodb_store/package.xml index 5e38a32..34fb9fd 100644 --- a/mongodb_store/package.xml +++ b/mongodb_store/package.xml @@ -12,42 +12,40 @@ https://github.com/strands-project/mongodb_store MIT - catkin - - rospy - roscpp - std_msgs - std_srvs - message_generation - mongodb_store_msgs - rostest - python-catkin-pkg - python3-catkin-pkg - mongodb - libmongoclient-dev - libssl-dev - topic_tools - + ament_python + + rospy + roscpp + std_msgs + std_srvs + mongodb_store_msgs + + + mongodb + + + + - geometry_msgs - - rospy - roscpp - std_msgs - std_srvs - python-future - python3-future - python-pymongo - python3-pymongo - mongodb - mongodb_store_msgs - libmongoclient-dev - topic_tools + + + + + + + + + + + + + + - geometry_msgs + - + ament_python diff --git a/mongodb_store/resource/mongodb_store b/mongodb_store/resource/mongodb_store new file mode 100644 index 0000000..e69de29 diff --git a/mongodb_store/setup.cfg b/mongodb_store/setup.cfg new file mode 100644 index 0000000..e7fbff9 --- /dev/null +++ b/mongodb_store/setup.cfg @@ -0,0 +1,4 @@ +[develop] +script_dir=$base/lib/mongodb_store +[install] +install_scripts=$base/lib/mongodb_store \ No newline at end of file diff --git a/mongodb_store/setup.py b/mongodb_store/setup.py index a1f6097..8f88b39 100644 --- a/mongodb_store/setup.py +++ b/mongodb_store/setup.py @@ -1,11 +1,62 @@ -## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD +import os +import typing +from glob import glob -from distutils.core import setup -from catkin_pkg.python_setup import generate_distutils_setup +from setuptools import find_packages, setup -# fetch values from package.xml -setup_args = generate_distutils_setup( - packages=['mongodb_store'], - package_dir={'': 'src'}) +package_name = "mongodb_store" -setup(**setup_args) + +def glob_files( + directory: str, + file_matcher: str = "*", + recursive=True, +) -> typing.Tuple[str, typing.List[str]]: + """ + Glob files in the given directory to use in the data files part of setup. + + Args: + directory: Directory to glob + file_matcher: Shell-style matching string used to match files to glob + recursive: Recurse over subdirectories. This probably doesn't work because subdirectories also get globbed + and setup doesn't like that. + + Returns: + Tuple of the directory in the share location, and list of globbed files + """ + return os.path.join("share", package_name, directory), glob( + os.path.join(directory, "" if not recursive else "**", file_matcher), + recursive=recursive, + ) + + +setup( + name=package_name, + version="2.0.3", + packages=find_packages(), + data_files=[ + ("share/ament_index/resource_index/packages", [f"resource/{package_name}"]), + (f"share/{package_name}", ["package.xml"]), + glob_files("launch", "*launch.[pxy][yma]*"), + ], + install_requires=["setuptools"], + zip_safe=True, + maintainer="Michal Staniaszek", + maintainer_email="michal@robots.ox.ac.uk", + description="MongoDB interaction for ROS2", + license="MIT", + tests_require=["pytest"], + entry_points={ + "console_scripts": [ + "config_manager = mongodb_store.scripts.config_manager:main", + "example_message_store_client = mongodb_store.scripts.example_message_store_client:main", + "example_multi_event_log = mongodb_store.scripts.example_multi_event_log:main", + "message_store_node = mongodb_store.scripts.message_store_node:main", + "mongo_bridge = mongodb_store.scripts.mongo_bridge:main", + "mongodb_play = mongodb_store.scripts.mongodb_play:main", + "mongodb_server = mongodb_store.scripts.mongodb_server:main", + "replicator_client= mongodb_store.scripts.replicator_client:main", + "replicator_node = mongodb_store.scripts.replicator_node:main", + ], + }, +) From 59963d61bef2199a78814c421f21edac6d403aa9 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Mon, 14 Apr 2025 16:29:05 +0100 Subject: [PATCH 11/25] preliminary conversion of server, message_store and util to ros2, not tested (but server appears to run) --- mongodb_store/mongodb_store/message_store.py | 290 +++++++++++---- .../mongodb_store/scripts/mongodb_server.py | 225 +++++++----- mongodb_store/mongodb_store/util.py | 344 ++++++++++-------- 3 files changed, 535 insertions(+), 324 deletions(-) diff --git a/mongodb_store/mongodb_store/message_store.py b/mongodb_store/mongodb_store/message_store.py index c752bfd..3add96b 100644 --- a/mongodb_store/mongodb_store/message_store.py +++ b/mongodb_store/mongodb_store/message_store.py @@ -1,6 +1,11 @@ -from __future__ import absolute_import -import rospy -import mongodb_store_msgs.srv as dc_srv +import rclpy +import typing +from mongodb_store_msgs.srv import ( + MongoInsertMsg, + MongoDeleteMsg, + MongoQueryMsg, + MongoUpdateMsg, +) import mongodb_store.util as dc_util from mongodb_store_msgs.msg import StringPair, StringPairList, SerialisedMessage, Insert from bson import json_util @@ -29,7 +34,14 @@ class MessageStoreProxy: """ - def __init__(self, service_prefix='/message_store', database='message_store', collection='message_store', queue_size=100): + def __init__( + self, + parent_node: rclpy.node.Node, + service_prefix="/message_store", + database="message_store", + collection="message_store", + queue_size=100, + ) -> None: """ Args: | service_prefix (str): The prefix to the *insert*, *update*, *delete* and @@ -37,37 +49,46 @@ def __init__(self, service_prefix='/message_store', database='message_store', co | database (str): The MongoDB database that this object works with. | collection (str): The MongoDB collect/on that this object works with. """ + self.parent_node = parent_node self.database = database self.collection = collection - insert_service = service_prefix + '/insert' - update_service = service_prefix + '/update' - delete_service = service_prefix + '/delete' - query_service = service_prefix + '/query_messages' + insert_service = service_prefix + "/insert" + update_service = service_prefix + "/update" + delete_service = service_prefix + "/delete" + query_service = service_prefix + "/query_messages" # try and get the mongo service, block until available - found_services_first_try = True # if found straight away - while not rospy.is_shutdown(): - try: - rospy.wait_for_service(insert_service,5) - rospy.wait_for_service(update_service,5) - rospy.wait_for_service(query_service,5) - rospy.wait_for_service(delete_service,5) - break - except rospy.ROSException as e: - found_services_first_try = False - rospy.logerr("Could not get message store services. Maybe the message " - "store has not been started? Retrying..") + found_services_first_try = True # if found straight away + self.insert_srv = self.parent_node.create_client(MongoInsertMsg, insert_service) + self.update_srv = self.parent_node.create_client(MongoUpdateMsg, update_service) + self.query_srv = self.parent_node.create_client(MongoQueryMsg, query_service) + self.delete_srv = self.parent_node.create_client(MongoDeleteMsg, delete_service) + + insert_topic = service_prefix + "/insert" + self.pub_insert = self.parent_node.create_publisher(Insert, insert_topic, 10) + + while rclpy.ok(): + try: + self.insert_srv.wait_for_service(5) + self.update_srv.wait_for_service(5) + self.query_srv.wait_for_service(5) + self.delete_srv.wait_for_service(5) + break + except Exception as e: + found_services_first_try = False + self.parent_node.get_logger().error( + "Could not get message store services. Maybe the message " + "store has not been started? Retrying..." + ) if not found_services_first_try: - rospy.loginfo("Message store services found.") - self.insert_srv = rospy.ServiceProxy(insert_service, dc_srv.MongoInsertMsg) - self.update_srv = rospy.ServiceProxy(update_service, dc_srv.MongoUpdateMsg) - self.query_srv = rospy.ServiceProxy(query_service, dc_srv.MongoQueryMsg) - self.delete_srv = rospy.ServiceProxy(delete_service, dc_srv.MongoDeleteMsg) - - insert_topic = service_prefix + '/insert' - self.pub_insert = rospy.Publisher(insert_topic, Insert, queue_size=queue_size) - - - def insert_named(self, name, message, meta = {}, wait=True): + self.parent_node.get_logger().info("Message store services found.") + + def insert_named( + self, + name: str, + message: "RosMessage", + meta: typing.Dict = None, + wait: bool = True, + ) -> str: """ Inserts a ROS message into the message storage, giving it a name for convenient later retrieval. @@ -83,12 +104,15 @@ def insert_named(self, name, message, meta = {}, wait=True): | (str) the ObjectId of the MongoDB document containing the stored message. """ # create a copy as we're modifying it + if meta is None: + meta = {} meta_copy = copy.copy(meta) meta_copy["name"] = name return self.insert(message, meta_copy, wait=wait) - - def insert(self, message, meta = {}, wait=True): + def insert( + self, message: "ROSMessage", meta: typing.Dict = None, wait: bool = True + ) -> typing.Union[bool, str]: """ Inserts a ROS message into the message storage. @@ -102,16 +126,32 @@ def insert(self, message, meta = {}, wait=True): """ # assume meta is a dict, convert k/v to tuple pairs - meta_tuple = (StringPair(dc_srv.MongoQueryMsgRequest.JSON_QUERY, json.dumps(meta, default=json_util.default)),) + meta_tuple = ( + StringPair( + first=MongoQueryMsg.Request.JSON_QUERY, + second=json.dumps(meta, default=json_util.default), + ), + ) serialised_msg = dc_util.serialise_message(message) + request = MongoInsertMsg.Request() + request.database = self.database + request.collection = self.collection + request.meta = StringPairList(pairs=meta_tuple) + request.message = serialised_msg + if wait: - return self.insert_srv(self.database, self.collection, serialised_msg, StringPairList(meta_tuple)).id + return self.insert_srv.call(request).id else: - msg = Insert(self.database, self.collection, serialised_msg, StringPairList(meta_tuple)) + msg = Insert( + database=self.database, + collection=self.collection, + message=serialised_msg, + meta=StringPairList(pairs=meta_tuple), + ) self.pub_insert.publish(msg) return True - def query_id(self, id, type): + def query_id(self, id: str, type: str): """ Finds and returns the message with the given ID. @@ -122,9 +162,9 @@ def query_id(self, id, type): | message (ROS message), meta (dict): The retrieved message and associated metadata or *None* if the named message could not be found. """ - return self.query(type, {'_id': ObjectId(id)}, {}, True) + return self.query(type, {"_id": ObjectId(id)}, {}, True) - def delete(self, message_id): + def delete(self, message_id: str) -> bool: """ Delete the message with the given ID. @@ -133,9 +173,20 @@ def delete(self, message_id): :Returns: | bool : was the object successfully deleted. """ - return self.delete_srv(self.database, self.collection, message_id) - - def query_named(self, name, type, single = True, meta = {}, limit = 0): + request = MongoDeleteMsg.Request() + request.database = self.database + request.collection = self.collection + request.document_id = message_id + return self.delete_srv.call(request) + + def query_named( + self, + name: str, + type: str, + single: bool = True, + meta: typing.Dict = None, + limit: int = 0, + ): """ Finds and returns the message(s) with the given name. @@ -144,18 +195,28 @@ def query_named(self, name, type, single = True, meta = {}, limit = 0): | type (str): The type of the stored message. | single (bool): Should only one message be returned? | meta (dict): Extra queries on the meta data of the message. - | limit (int): Limit number of return documents + | limit (int): Limit number of return documents :Return: | message (ROS message), meta (dict): The retrieved message and associated metadata or *None* if the named message could not be found. """ # create a copy as we're modifying it + if meta is None: + meta = {} meta_copy = copy.copy(meta) meta_copy["name"] = name - return self.query(type, {}, meta_copy, single, [], limit) - - def update_named(self, name, message, meta = {}, upsert = False): + return self.query( + type, {}, meta_copy, single, [], projection_query={}, limit=limit + ) + + def update_named( + self, + name: str, + message: "ROSMessage", + meta: typing.Dict = None, + upsert: bool = False, + ) -> typing.Tuple[str, bool]: """ Updates a named message. @@ -177,27 +238,34 @@ def update_named(self, name, message, meta = {}, upsert = False): return self.update(message, meta_copy, {}, meta_query, upsert) - def update_id(self, id, message, meta = {}, upsert = False): + def update_id(self, id, message, meta=None, upsert=False): """ Updates a message by MongoDB ObjectId. - :Args: - | id (str): The MongoDB ObjectId of the doucment storing the message. - | message (ROS Message): The updated ROS message - | meta (dict): Updated meta data to store with the message. - | upsert (bool): If True, insert the named message if it doesnt exist. - :Return: - | str, bool: The MongoDB ObjectID of the document, and whether it was altered by + Args: + id: The MongoDB ObjectId of the doucment storing the message. + message: The updated ROS message + meta: Updated meta data to store with the message. + upsert: If True, insert the named message if it doesnt exist. + Return: + str, bool: The MongoDB ObjectID of the document, and whether it was altered by the update. """ - msg_query = {'_id': ObjectId(id)} + msg_query = {"_id": ObjectId(id)} meta_query = {} return self.update(message, meta, msg_query, meta_query, upsert) - def update(self, message, meta = {}, message_query = {}, meta_query = {}, upsert = False): + def update( + self, + message: "ROSMessage", + meta: typing.Dict = None, + message_query: typing.Dict = None, + meta_query: typing.Dict = None, + upsert: bool = False, + ): """ Updates a message. @@ -212,17 +280,58 @@ def update(self, message, meta = {}, message_query = {}, meta_query = {}, upser the update. """ - # serialise the json queries to strings using json_util.dumps - message_query_tuple = (StringPair(dc_srv.MongoQueryMsgRequest.JSON_QUERY, json.dumps(message_query, default=json_util.default)),) - meta_query_tuple = (StringPair(dc_srv.MongoQueryMsgRequest.JSON_QUERY, json.dumps(meta_query, default=json_util.default)),) - meta_tuple = (StringPair(dc_srv.MongoQueryMsgRequest.JSON_QUERY, json.dumps(meta, default=json_util.default)),) - return self.update_srv(self.database, self.collection, upsert, StringPairList(message_query_tuple), StringPairList(meta_query_tuple), dc_util.serialise_message(message), StringPairList(meta_tuple)) + if message_query is None: + message_query = {} + if meta_query is None: + meta_query = {} + if meta is None: + meta = {} + # serialise the json queries to strings using json_util.dumps + message_query_tuple = ( + StringPair( + first=MongoQueryMsg.Request.JSON_QUERY, + second=json.dumps(message_query, default=json_util.default), + ), + ) + meta_query_tuple = ( + StringPair( + first=MongoQueryMsg.Request.JSON_QUERY, + second=json.dumps(meta_query, default=json_util.default), + ), + ) + meta_tuple = ( + StringPair( + first=MongoQueryMsg.Request.JSON_QUERY, + second=json.dumps(meta, default=json_util.default), + ), + ) + + request = MongoUpdateMsg.Request() + request.database = self.database + request.collection = self.collection + request.upsert = upsert + request.message_query = StringPairList(pairs=message_query_tuple) + request.meta_query = StringPairList(pairs=meta_query_tuple) + request.message = dc_util.serialise_message(message) + request.meta = StringPairList(pairs=meta_tuple) + + return self.update_srv.call(request) """ Returns [message, meta] where message is the queried message and meta a dictionary of meta information. If single is false returns a list of these lists. """ - def query(self, type, message_query = {}, meta_query = {}, single = False, sort_query = [], projection_query = {}, limit=0): + + def query( + self, + type: str, + message_query: typing.Dict = None, + meta_query: typing.Dict = None, + single: bool = False, + sort_query: typing.List[typing.Tuple] = None, + projection_query: typing.Dict = None, + limit: int = 0, + ): """ Finds and returns message(s) matching the message and meta data queries. @@ -238,24 +347,57 @@ def query(self, type, message_query = {}, meta_query = {}, single = False, sort_ | [message, meta] where message is the queried message and meta a dictionary of meta information. If single is false returns a list of these lists. """ + if message_query is None: + message_query = {} + if meta_query is None: + meta_query = {} + if sort_query is None: + sort_query = [] + if projection_query is None: + projection_query = {} + # assume meta is a dict, convert k/v to tuple pairs for ROS msg type # serialise the json queries to strings using json_util.dumps - message_tuple = (StringPair(dc_srv.MongoQueryMsgRequest.JSON_QUERY, json.dumps(message_query, default=json_util.default)),) - meta_tuple = (StringPair(dc_srv.MongoQueryMsgRequest.JSON_QUERY, json.dumps(meta_query, default=json_util.default)),) - projection_tuple =(StringPair(dc_srv.MongoQueryMsgRequest.JSON_QUERY, json.dumps(projection_query, default=json_util.default)),) + + message_tuple = ( + StringPair( + first=MongoQueryMsg.Request.JSON_QUERY, + second=json.dumps(message_query, default=json_util.default), + ), + ) + meta_tuple = ( + StringPair( + first=MongoQueryMsg.Request.JSON_QUERY, + second=json.dumps(meta_query, default=json_util.default), + ), + ) + projection_tuple = ( + StringPair( + first=MongoQueryMsg.Request.JSON_QUERY, + second=json.dumps(projection_query, default=json_util.default), + ), + ) if len(sort_query) > 0: - sort_tuple = [StringPair(str(k), str(v)) for k, v in sort_query] + sort_tuple = [ + StringPair(first=str(k), second=str(v)) for k, v in sort_query + ] else: - sort_tuple = [] + sort_tuple = [] + + request = MongoQueryMsg.Request() + request.database = self.database + request.collection = self.collection + request.type = type + request.single = single + request.limit = limit + request.message_query = message_query + request.meta_query = meta_query + request.projection_query = projection_query + request.sort_query = sort_query - response = self.query_srv( - self.database, self.collection, type, single, limit, - StringPairList(message_tuple), - StringPairList(meta_tuple), - StringPairList(sort_tuple), - StringPairList(projection_tuple)) + response = self.query_srv.call(request) if response.messages is None: messages = [] @@ -270,4 +412,4 @@ def query(self, type, message_query = {}, meta_query = {}, single = False, sort_ else: return [None, None] else: - return list(zip(messages,metas)) + return list(zip(messages, metas)) diff --git a/mongodb_store/mongodb_store/scripts/mongodb_server.py b/mongodb_store/mongodb_store/scripts/mongodb_server.py index a7ec139..efbb8d9 100755 --- a/mongodb_store/mongodb_store/scripts/mongodb_server.py +++ b/mongodb_store/mongodb_store/scripts/mongodb_server.py @@ -1,19 +1,16 @@ -#!/usr/bin/env python -from __future__ import absolute_import -import rospy +import rclpy import subprocess import sys import os import re -import signal import errno -from std_srvs.srv import Empty, EmptyResponse +from rclpy.duration import Duration +import threading + +from rcl_interfaces.msg import ParameterDescriptor +from std_srvs.srv import Empty import shutil -import platform -if float(platform.python_version()[0:2]) >= 3.0: - _PY3 = True -else: - _PY3 = False +import pymongo import mongodb_store.util @@ -22,42 +19,49 @@ MongoClient = mongodb_store.util.import_MongoClient() -import pymongo def is_socket_free(host, port): - import socket; + import socket + sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) result = sock.connect_ex((host, port)) return result != 0 -class MongoServer(object): - def __init__(self): - rospy.init_node("mongodb_server", anonymous=True)#, disable_signals=True) +class MongoServer(rclpy.node.Node): + def __init__(self): + # TODO: This should be anonymous, but ROS2 doesn't allow for that natively + super().__init__("mongodb_server") # Has the db already gone down, before the ros node? self._gone_down = False - self._ready = False # is the db ready: when mongo says "waiting for connection" - - - self.test_mode = rospy.get_param("~test_mode", False) - self.repl_set = rospy.get_param("~repl_set", None) - self.bind_to_host = rospy.get_param("~bind_to_host", False) + self._ready = False # is the db ready: when mongo says "waiting for connection" + self.test_mode = self.declare_parameter( + "test_mode", False, descriptor=ParameterDescriptor(description="") + ).value + self.repl_set = self.declare_parameter( + "repl_set", None, descriptor=ParameterDescriptor(description="") + ).value + self.bind_to_host = self.declare_parameter( + "bind_to_host", False, descriptor=ParameterDescriptor(description="") + ).value if self.test_mode: import random default_host = "localhost" - default_port = random.randrange(49152,65535) + default_port = random.randrange(49152, 65535) count = 0 while not is_socket_free(default_host, default_port): - default_port = random.randrange(49152,65535) + default_port = random.randrange(49152, 65535) count += 1 if count > 100: - rospy.logerr("Can't find a free port to run the test server on.") + self.get_logger().error( + "Can't find a free port to run the test server on." + ) sys.exit(1) self.default_path = "/tmp/ros_mongodb_store_%d" % default_port @@ -68,53 +72,81 @@ def __init__(self): self.default_path = "/opt/ros/mongodb_store" # Get the database path - self._db_path = rospy.get_param("~database_path", self.default_path) - is_master = rospy.get_param("~master", True) + self._db_path = self.declare_parameter( + "database_path", + self.default_path, + descriptor=ParameterDescriptor(description=""), + ).value + is_master = self.declare_parameter( + "master", True, descriptor=ParameterDescriptor(description="") + ).value if is_master: - self._mongo_host = rospy.get_param("mongodb_host", default_host) - rospy.set_param("mongodb_host",self._mongo_host) - self._mongo_port = rospy.get_param("mongodb_port", default_port) - rospy.set_param("mongodb_port",self._mongo_port) + # TODO: These used to be global params + self._mongo_host = self.declare_parameter( + "mongodb_host", + default_host, + descriptor=ParameterDescriptor(description=""), + ).value + self._mongo_port = self.declare_parameter( + "mongodb_port", + default_port, + descriptor=ParameterDescriptor(description=""), + ).value else: - self._mongo_host = rospy.get_param("~host") - self._mongo_port = rospy.get_param("~port") + self._mongo_host = self.declare_parameter( + "host", descriptor=ParameterDescriptor(description="") + ).value + self._mongo_port = self.declare_parameter( + "port", descriptor=ParameterDescriptor(description="") + ).value - rospy.loginfo("Mongo server address: "+self._mongo_host+":"+str(self._mongo_port)) + self.get_logger().info( + "Mongo server address: " + self._mongo_host + ":" + str(self._mongo_port) + ) # Check that mongodb is installed try: - mongov = subprocess.check_output(["mongod","--version"]) - match = re.search("db version v(\d+\.\d+\.\d+)", mongov.decode('utf-8')) - self._mongo_version=match.group(1) + mongov = subprocess.check_output(["mongod", "--version"]) + match = re.search("db version v(\d+\.\d+\.\d+)", mongov.decode("utf-8")) + self._mongo_version = match.group(1) except subprocess.CalledProcessError: - rospy.logerr("Can't find MongoDB executable. Is it installed?\nInstall it with \"sudo apt-get install mongodb\"") + self.get_logger().error( + 'Can\'t find MongoDB executable. Is it installed?\nInstall it with "sudo apt install mongodb"' + ) sys.exit(1) - rospy.loginfo("Found MongoDB version " + self._mongo_version) + self.get_logger().info("Found MongoDB version " + self._mongo_version) # Check that the provided db path exists. if not os.path.exists(self._db_path): - rospy.logerr("Can't find database at supplied path " + self._db_path + ". If this is a new DB, create it as an empty directory.") + self.get_logger().error( + "Can't find database at supplied path " + + self._db_path + + ". If this is a new DB, create it as an empty directory." + ) sys.exit(1) # Advertise ros services for db interaction - self._shutdown_srv = rospy.Service("/datacentre/shutdown", Empty, self._shutdown_srv_cb) - self._wait_ready_srv = rospy.Service("/datacentre/wait_ready",Empty,self._wait_ready_srv_cb) - - rospy.on_shutdown(self._on_node_shutdown) + self._shutdown_srv = self.create_service( + Empty, "/datacentre/shutdown", self._shutdown_srv_cb + ) + self._wait_ready_srv = self.create_service( + Empty, "/datacentre/wait_ready", self._wait_ready_srv_cb + ) - # Start the mongodb server - self._mongo_loop() + self.mongo_thread = threading.Thread(target=self._mongo_loop) + self.mongo_thread.start() def _mongo_loop(self): # Blocker to prevent Ctrl-C being passed to the mongo server def block_mongo_kill(): os.setpgrp() -# signal.signal(signal.SIGINT, signal.SIG_IGN) - #cmd = ["mongod","--dbpath",self._db_path,"--port",str(self._mongo_port),"--smallfiles","--bind_ip","127.0.0.1"] - cmd = ["mongod","--dbpath",self._db_path,"--port",str(self._mongo_port)] + # signal.signal(signal.SIGINT, signal.SIG_IGN) + + # cmd = ["mongod","--dbpath",self._db_path,"--port",str(self._mongo_port),"--smallfiles","--bind_ip","127.0.0.1"] + cmd = ["mongod", "--dbpath", self._db_path, "--port", str(self._mongo_port)] if self.bind_to_host: cmd.append("--bind_ip") @@ -122,53 +154,82 @@ def block_mongo_kill(): else: cmd.append("--bind_ip") cmd.append("0.0.0.0") - if self.repl_set is not None: cmd.append("--replSet") cmd.append(self.repl_set) - self._mongo_process = subprocess.Popen(cmd, - stdout=subprocess.PIPE, - preexec_fn = block_mongo_kill) + self._mongo_process = subprocess.Popen( + cmd, stdout=subprocess.PIPE, preexec_fn=block_mongo_kill + ) - while self._mongo_process.poll() is None:# and not rospy.is_shutdown(): + while self._mongo_process.poll() is None: # and not rospy.is_shutdown(): try: - stdout = self._mongo_process.stdout.readline().decode('utf-8') - except IOError as e: # probably interupt because shutdown cut it up + stdout = self._mongo_process.stdout.readline().decode("utf-8") + except IOError as e: # probably interupt because shutdown cut it up if e.errno == errno.EINTR: continue else: raise if stdout is not None: if stdout.find("ERROR") != -1: - rospy.logerr(stdout.strip()) + self.get_logger().error(stdout.strip()) else: - rospy.loginfo(stdout.strip()) + self.get_logger().info(stdout.strip()) - if stdout.find("waiting for connections on port") !=-1: - self._ready=True + if stdout.find("waiting for connections on port") != -1: + self._ready = True if self.repl_set is not None: try: self.initialize_repl_set() except Exception as e: - rospy.logwarn("initialzing replSet failed: %s" % e) + self.get_logger().warning("initialzing replSet failed: %s" % e) - if not rospy.is_shutdown(): - rospy.logerr("MongoDB process stopped!") + if not rclpy.ok(): + self.get_logger().error("MongoDB process stopped!") - if self._mongo_process.returncode!=0: - rospy.logerr("Mongo process error! Exit code="+str(self._mongo_process.returncode)) + if self._mongo_process.returncode != 0: + self.get_logger().error( + "Mongo process error! Exit code=" + str(self._mongo_process.returncode) + ) self._gone_down = True - def _on_node_shutdown(self): - rospy.loginfo("Shutting down datacentre") - self._ready=False - if self._gone_down: - rospy.logwarn("It looks like Mongo already died. Watch out as the DB might need recovery time at next run.") + def _shutdown_srv_cb(self, req): + self.destroy_node() + rclpy.shutdown() + return Empty.Response() + + def _wait_ready_srv_cb(self, req): + while not self._ready: + self.get_clock().sleep_for(Duration(seconds=0.1)) + return Empty.Response() + + def initialize_repl_set(self): + c = pymongo.Connection( + "%s:%d" % (self._mongo_host, self._mongo_port), slave_okay=True + ) + c.admin.command("replSetInitiate") + c.close() + + +def main(): + rclpy.init() + try: + server = MongoServer() + rclpy.spin(server) + server.destroy_node() + rclpy.shutdown() + finally: + # TODO: The context on_shutdown doesn't seem to work, so moving that code here + server.get_logger().info("Shutting down datacentre") + server._ready = False + if server._gone_down: + server.get_logger().warning( + "It looks like Mongo already died. Watch out as the DB might need recovery time at next run." + ) return try: - c = MongoClient(host=self._mongo_host, port=self._mongo_port) + c = MongoClient(host=server._mongo_host, port=server._mongo_port) except pymongo.errors.ConnectionFailure: c = None try: @@ -177,26 +238,8 @@ def _on_node_shutdown(self): except pymongo.errors.AutoReconnect: pass - if self.test_mode: # remove auto-created DB in the /tmp folder + if server.test_mode: # remove auto-created DB in the /tmp folder try: - shutil.rmtree(self.default_path) + shutil.rmtree(server.default_path) except Exception as e: - rospy.logerr(e) - - def _shutdown_srv_cb(self,req): - rospy.signal_shutdown("Shutdown request..") - return EmptyResponse() - - def _wait_ready_srv_cb(self,req): - while not self._ready: - rospy.sleep(0.1) - return EmptyResponse() - - def initialize_repl_set(self): - c = pymongo.Connection("%s:%d" % (self._mongo_host,self._mongo_port), slave_okay=True) - c.admin.command("replSetInitiate") - c.close() - -if __name__ == '__main__': - server = MongoServer() - + server.get_logger().error(e) diff --git a/mongodb_store/mongodb_store/util.py b/mongodb_store/mongodb_store/util.py index 7d0f571..7a42052 100644 --- a/mongodb_store/mongodb_store/util.py +++ b/mongodb_store/mongodb_store/util.py @@ -1,28 +1,23 @@ -from __future__ import print_function, absolute_import -import rospy -import genpy -from std_srvs.srv import Empty -import yaml -from bson import json_util, Binary +import rclpy +import rclpy.type_support +import rclpy.action +import importlib import json +from datetime import datetime +from datetime import timezone +from io import BytesIO as Buffer -import copy -import platform -if float(platform.python_version()[0:2]) >= 3.0: - _PY3 = True - from io import BytesIO as Buffer -else: - _PY3 = False - from StringIO import StringIO as Buffer -from mongodb_store_msgs.msg import SerialisedMessage -from mongodb_store_msgs.srv import MongoQueryMsgRequest - +from bson import json_util, Binary from pymongo.errors import ConnectionFailure +from std_srvs.srv import Empty + +from mongodb_store_msgs.msg import SerialisedMessage +from mongodb_store_msgs.srv import MongoQueryMsg -import importlib -from datetime import datetime -def check_connection_to_mongod(db_host, db_port, connection_string=None): +def check_connection_to_mongod( + parent_node: rclpy.node.Node, db_host, db_port, connection_string=None +) -> bool: """ Check connection to mongod server @@ -31,34 +26,30 @@ def check_connection_to_mongod(db_host, db_port, connection_string=None): """ if check_for_pymongo(): try: - try: - # pymongo 2.X - from pymongo import Connection - Connection(db_host, db_port) - return True - except: - # pymongo 3.X - from pymongo import MongoClient - if connection_string is None: - client = MongoClient(db_host, db_port, connect=False) - else: - client = MongoClient(connection_string) - result = client.admin.command('ismaster') - return True + # pymongo 3.X + from pymongo import MongoClient + + if connection_string is None: + client = MongoClient(db_host, db_port, connect=False) + else: + client = MongoClient(connection_string) + result = client.admin.command("ismaster") + return True except ConnectionFailure: if connection_string is None: - rospy.logerr("Could not connect to mongo server %s:%d" % (db_host, db_port)) - rospy.logerr("Make sure mongod is launched on your specified host/port") + parent_node.get_logger().error( + f"Could not connect to mongo server {db_host}:{db_port}\nMake sure mongod is launched on your specified host/port" + ) else: - rospy.logerr("Could not connect to mongo server %s" % (connection_string)) - rospy.logerr("Make sure mongod is launched on your specified host/port") - + parent_node.get_logger().error( + f"Could not connect to mongo server {connection_string}\nMake sure mongod is launched on your specified host/port" + ) return False else: return False -def wait_for_mongo(timeout=60, ns="/datacentre"): +def wait_for_mongo(parent_node: rclpy.node.Node, timeout=60, ns="/datacentre"): """ Waits for the mongo server, as started through the mongodb_store/mongodb_server.py wrapper @@ -66,15 +57,19 @@ def wait_for_mongo(timeout=60, ns="/datacentre"): | bool : True on success, False if server not even started. """ # Check that mongo is live, create connection - try: - rospy.wait_for_service(ns + "/wait_ready", timeout) - except rospy.exceptions.ROSException as e: - rospy.logerr("Can't connect to MongoDB server. Make sure mongodb_store/mongodb_server.py node is started.") + service = ns + "/wait_ready" + client = parent_node.create_client(Empty, service) + + valid = client.wait_for_service(timeout) + if not valid: + parent_node.get_logger().error( + "Can't connect to MongoDB server. Make sure mongodb_store/mongodb_server.py node is started." + ) return False - wait = rospy.ServiceProxy(ns + '/wait_ready', Empty) - wait() + client.call(Empty.Request()) return True + def check_for_pymongo(): """ Checks for required version of pymongo python library. @@ -85,61 +80,67 @@ def check_for_pymongo(): try: import pymongo except: - rospy.logerr("ERROR!!!") - rospy.logerr("Can't import pymongo, this is needed by mongodb_store.") - rospy.logerr("Make sure it is installed (sudo pip install pymongo)") + print("ERROR!!!") + print( + "Can't import pymongo, this is needed by mongodb_store." + ) + print( + "Make sure it is installed (pip install pymongo)" + ) return False return True -""" -Pick an object to use as MongoClient based on the currently installed pymongo -version. Use this instead of importing Connection or MongoClient from pymongo -directly. -Example: - MongoClient = util.importMongoClient() -""" def import_MongoClient(): + """ + Pick an object to use as MongoClient based on the currently installed pymongo + version. Use this instead of importing Connection or MongoClient from pymongo + directly. + + Example: + MongoClient = util.importMongoClient() + """ import pymongo - if pymongo.version >= '2.4': + + if pymongo.version >= "2.4": + def mongo_client_wrapper(*args, **kwargs): return pymongo.MongoClient(*args, **kwargs) + return mongo_client_wrapper - else: - import functools - def mongo_client_wrapper(*args, **kwargs): - return pymongo.Connection(*args, **kwargs) - return functools.partial(mongo_client_wrapper, safe=True) -""" -Given a ROS msg and a dictionary of the right values, fill in the msg -""" -def _fill_msg(msg,dic): +def _fill_msg(msg, dic): + """ + Given a ROS msg and a dictionary of the right values, fill in the msg + """ for i in dic: - if isinstance(dic[i],dict): - _fill_msg(getattr(msg,i),dic[i]) + if isinstance(dic[i], dict): + _fill_msg(getattr(msg, i), dic[i]) else: - setattr(msg,i,dic[i]) + setattr(msg, i, dic[i]) -""" -Given a document in the database, return metadata and ROS message -- must have been -""" def document_to_msg_and_meta(document, TYPE): + """ + Given a document in the database, return metadata and ROS message -- must have been + """ meta = document["_meta"] msg = TYPE() - _fill_msg(msg,document["msg"]) - return meta,msg + _fill_msg(msg, document["msg"]) + return meta, msg + + + -""" -Given a document return ROS message -""" def document_to_msg(document, TYPE): + """ + Given a document return ROS message + """ msg = TYPE() - _fill_msg(msg,document) - return meta + _fill_msg(msg, document) + return msg def msg_to_document(msg): @@ -157,23 +158,20 @@ def msg_to_document(msg): | dict : A dictionary representation of the supplied message. """ - - - d = {} slot_types = [] - if hasattr(msg,'_slot_types'): - slot_types = msg._slot_types + if hasattr(msg, "SLOT_TYPES"): + slot_types = msg.SLOT_TYPES else: slot_types = [None] * len(msg.__slots__) - - for (attr, type) in zip(msg.__slots__, slot_types): + for attr, type in zip(msg._fields_and_string_types.keys(), slot_types): d[attr] = sanitize_value(attr, getattr(msg, attr), type) return d + def sanitize_value(attr, v, type): """ De-rosify a msg. @@ -189,35 +187,26 @@ def sanitize_value(attr, v, type): | A sanitized version of v. """ - # print '---' - # print attr - # print v.__class__ - # print type - # print v + # print '---' + # print attr + # print v.__class__ + # print type + # print v if isinstance(v, str): - if type == 'uint8[]': + if type == "uint8[]": v = Binary(v) - else: - # ensure unicode - try: - if not _PY3: # All strings are unicode in Python 3 - v = unicode(v, "utf-8") - except UnicodeDecodeError as e: - # at this point we can deal with the encoding, so treat it as binary - v = Binary(v) + # no need to carry on with the other type checks below return v - if isinstance(v, rospy.Message): - return msg_to_document(v) - elif isinstance(v, genpy.rostime.Time): + + if rclpy.type_support.check_for_type_support(v): + # This should be a sufficient check for whether something is a ros msg, srv, or action return msg_to_document(v) - elif isinstance(v, genpy.rostime.Duration): - return msg_to_document(v) elif isinstance(v, list): result = [] for t in v: - if hasattr(t, '_type'): + if hasattr(t, "_type"): result.append(sanitize_value(None, t, t._type)) else: result.append(sanitize_value(None, t, None)) @@ -226,8 +215,6 @@ def sanitize_value(attr, v, type): return v - - def store_message(collection, msg, meta, oid=None): """ Update ROS message into the DB @@ -240,23 +227,28 @@ def store_message(collection, msg, meta, oid=None): :Returns: | str: ObjectId of the MongoDB document. """ - doc=msg_to_document(msg) - doc["_meta"]=meta + doc = msg_to_document(msg) + doc["_meta"] = meta # also store type information doc["_meta"]["stored_class"] = msg.__module__ + "." + msg.__class__.__name__ doc["_meta"]["stored_type"] = msg._type - if msg._type == "soma2_msgs/SOMA2Object" or msg._type == "soma_msgs/SOMAObject" or msg._type == "soma_msgs/SOMAROIObject": - add_soma_fields(msg,doc) + if ( + msg._type == "soma2_msgs/SOMA2Object" + or msg._type == "soma_msgs/SOMAObject" + or msg._type == "soma_msgs/SOMAROIObject" + ): + add_soma_fields(msg, doc) - if hasattr(msg, '_connection_header'): - print(getattr(msg, '_connection_header')) + if hasattr(msg, "_connection_header"): + print(getattr(msg, "_connection_header")) if oid != None: doc["_id"] = oid return collection.insert(doc) + # """ # Stores a ROS message into the DB with msg and meta as separate fields # """ @@ -267,7 +259,6 @@ def store_message(collection, msg, meta, oid=None): # return collection.insert(doc) - def store_message_no_meta(collection, msg): """ Store a ROS message sans meta data. @@ -278,7 +269,7 @@ def store_message_no_meta(collection, msg): :Returns: | str: The ObjectId of the MongoDB document created. """ - doc=msg_to_document(msg) + doc = msg_to_document(msg) return collection.insert(doc) @@ -308,19 +299,21 @@ def fill_message(message, document): z: 0.0 w: 0.0 """ - for slot, slot_type in zip(message.__slots__, - getattr(message,"_slot_types",[""]*len(message.__slots__))): + for slot, slot_type in zip( + message.__slots__, + getattr(message, "SLOT_TYPES", [""] * len(message.__slots__)), + ): # This check is required since objects returned with projection queries can have absent keys if slot in document.keys(): value = document[slot] - # fill internal structures if value is a dictionary itself + # fill internal structures if value is a dictionary itself if isinstance(value, dict): fill_message(getattr(message, slot), value) - elif isinstance(value, list) and slot_type.find("/")!=-1: - # if its a list and the type is some message (contains a "/") - lst=[] - # Remove [] from message type ([:-2]) + elif isinstance(value, list) and slot_type.find("/") != -1: + # if its a list and the type is some message (contains a "/") + lst = [] + # Remove [] from message type ([:-2]) msg_type = type_to_class_string(slot_type[:-2]) msg_class = load_class(msg_type) for i in value: @@ -329,10 +322,8 @@ def fill_message(message, document): lst.append(msg) setattr(message, slot, lst) else: - if not _PY3 and isinstance(value, unicode): # All strings are unicode in Python 3 - setattr(message, slot, value.encode('utf-8')) - else: - setattr(message, slot, value) + setattr(message, slot, value) + def dictionary_to_message(dictionary, cls): """ @@ -367,7 +358,10 @@ def dictionary_to_message(dictionary, cls): return message -def query_message(collection, query_doc, sort_query=[], projection_query={},find_one=False, limit=0): + +def query_message( + collection, query_doc, sort_query=None, projection_query=None, find_one=False, limit=0 +): """ Peform a query for a stored messages, returning results in list. @@ -381,32 +375,51 @@ def query_message(collection, query_doc, sort_query=[], projection_query={},find :Returns: | dict or list of dict: the MongoDB document(s) found by the query """ - + if sort_query is None: + sort_query = [] + if projection_query is None: + projection_query = {} if find_one: ids = () if sort_query: if not projection_query: result = collection.find_one(query_doc, sort=sort_query) else: - result = collection.find_one(query_doc, projection_query, sort=sort_query) + result = collection.find_one( + query_doc, projection_query, sort=sort_query + ) elif projection_query: result = collection.find_one(query_doc, projection_query) else: result = collection.find_one(query_doc) if result: - return [ result ] + return [result] else: return [] else: if sort_query: - if not projection_query: - return [ result for result in collection.find(query_doc).sort(sort_query).limit(limit) ] + if not projection_query: + return [ + result + for result in collection.find(query_doc) + .sort(sort_query) + .limit(limit) + ] else: - return [ result for result in collection.find(query_doc, projection_query).sort(sort_query).limit(limit) ] + return [ + result + for result in collection.find(query_doc, projection_query) + .sort(sort_query) + .limit(limit) + ] elif projection_query: - return [ result for result in collection.find(query_doc, projection_query).limit(limit) ] + return [ + result + for result in collection.find(query_doc, projection_query).limit(limit) + ] else: - return [ result for result in collection.find(query_doc).limit(limit) ] + return [result for result in collection.find(query_doc).limit(limit)] + def update_message(collection, query_doc, msg, meta, upsert): """ @@ -433,15 +446,19 @@ def update_message(collection, query_doc, msg, meta, upsert): return "", False # convert msg to db document - doc=msg_to_document(msg) + doc = msg_to_document(msg) - if msg._type == "soma2_msgs/SOMA2Object" or msg._type == "soma_msgs/SOMAObject" or msg._type == "soma_msgs/SOMAROIObject": - add_soma_fields(msg,doc) + if ( + msg._type == "soma2_msgs/SOMA2Object" + or msg._type == "soma_msgs/SOMAObject" + or msg._type == "soma_msgs/SOMAROIObject" + ): + add_soma_fields(msg, doc) - #update _meta + # update _meta doc["_meta"] = result["_meta"] - #merge the two dicts, overwiriting elements in doc["_meta"] with elements in meta - doc["_meta"]=dict(list(doc["_meta"].items()) + list(meta.items())) + # merge the two dicts, overwiriting elements in doc["_meta"] with elements in meta + doc["_meta"] = dict(list(doc["_meta"].items()) + list(meta.items())) # ensure necessary parts are there too doc["_meta"]["stored_class"] = msg.__module__ + "." + msg.__class__.__name__ @@ -464,10 +481,11 @@ def query_message_ids(collection, query_doc, find_one): if find_one: result = collection.find_one(query_doc) if result: - return str(result["_id"]), + return (str(result["_id"]),) else: - return tuple(str(result["_id"]) for result in collection.find(query_doc, {'_id':1})) - + return tuple( + str(result["_id"]) for result in collection.find(query_doc, {"_id": 1}) + ) def type_to_class_string(type): @@ -484,10 +502,11 @@ def type_to_class_string(type): :Returns: | str: A python class string for the ROS message type supplied """ - parts = type.split('/') + parts = type.split("/") cls_string = "%s.msg._%s.%s" % (parts[0], parts[1], parts[1]) return cls_string + def load_class(full_class_string): """ Dynamically load a class from a string @@ -523,6 +542,7 @@ def serialise_message(message): serialised_msg.type = message._type return serialised_msg + def deserialise_message(serialised_message): """ Create a ROS message from a mongodb_store_msgs/SerialisedMessage @@ -552,6 +572,7 @@ def string_pair_list_to_dictionary_no_json(spl): """ return dict((pair.first, pair.second) for pair in spl) + def string_pair_list_to_dictionary(spl): """ Creates a dictionary from a mongodb_store_msgs/StringPairList which could contain JSON as a string. @@ -562,39 +583,44 @@ def string_pair_list_to_dictionary(spl): :Returns: | dict: resulting dictionary """ - if len(spl.pairs) > 0 and spl.pairs[0].first == MongoQueryMsgRequest.JSON_QUERY: + if len(spl.pairs) > 0 and spl.pairs[0].first == MongoQueryMsg.Request.JSON_QUERY: # print "looks like %s", spl.pairs[0].second return json.loads(spl.pairs[0].second, object_hook=json_util.object_hook) # else use the string pairs else: return string_pair_list_to_dictionary_no_json(spl.pairs) + def topic_name_to_collection_name(topic_name): """ Converts the fully qualified name of a topic into legal mongodb collection name. """ return topic_name.replace("/", "_")[1:] -def add_soma_fields(msg,doc): + +def add_soma_fields(msg, doc): """ For soma Object msgs adds the required fields as indexes to the mongodb object. """ - if hasattr(msg, 'pose'): - doc["loc"] = [doc["pose"]["position"]["x"],doc["pose"]["position"]["y"]] - if hasattr(msg,'logtimestamp'): - doc["timestamp"] = datetime.utcfromtimestamp(doc["logtimestamp"]) -#doc["timestamp"] = datetime.strptime(doc["logtime"], "%Y-%m-%dT%H:%M:%SZ") + if hasattr(msg, "pose"): + doc["loc"] = [doc["pose"]["position"]["x"], doc["pose"]["position"]["y"]] + if hasattr(msg, "logtimestamp"): + doc["timestamp"] = datetime.fromtimestamp(doc["logtimestamp"], timezone.utc) + # doc["timestamp"] = datetime.strptime(doc["logtime"], "%Y-%m-%dT%H:%M:%SZ") - if hasattr(msg, 'geotype'): - if(doc["geotype"] == "Point"): + if hasattr(msg, "geotype"): + if doc["geotype"] == "Point": for p in doc["geoposearray"]["poses"]: - doc["geoloc"] = {'type': doc['geotype'],'coordinates': [p["position"]["x"], p["position"]["y"]]} - if(msg._type =="soma_msgs/SOMAROIObject"): + doc["geoloc"] = { + "type": doc["geotype"], + "coordinates": [p["position"]["x"], p["position"]["y"]], + } + if msg._type == "soma_msgs/SOMAROIObject": coordinates = [] doc["geotype"] = "Polygon" for p in doc["geoposearray"]["poses"]: coordinates.append([p["position"]["x"], p["position"]["y"]]) - coordinates2=[] + coordinates2 = [] coordinates2.append(coordinates) - doc["geoloc"] = {'type': doc['geotype'],'coordinates': coordinates2} + doc["geoloc"] = {"type": doc["geotype"], "coordinates": coordinates2} From 9290290ebc9d24dac294a707281d4e2d8f38f571 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Tue, 15 Apr 2025 09:46:08 +0100 Subject: [PATCH 12/25] shutdown works properly, fix string check so ready attribute is correctly set when startup completes --- .../mongodb_store/scripts/mongodb_server.py | 35 ++++++++++++------- 1 file changed, 23 insertions(+), 12 deletions(-) diff --git a/mongodb_store/mongodb_store/scripts/mongodb_server.py b/mongodb_store/mongodb_store/scripts/mongodb_server.py index efbb8d9..aa5c5c9 100755 --- a/mongodb_store/mongodb_store/scripts/mongodb_server.py +++ b/mongodb_store/mongodb_store/scripts/mongodb_server.py @@ -8,6 +8,7 @@ import threading from rcl_interfaces.msg import ParameterDescriptor +from rclpy.executors import MultiThreadedExecutor from std_srvs.srv import Empty import shutil import pymongo @@ -120,9 +121,8 @@ def __init__(self): # Check that the provided db path exists. if not os.path.exists(self._db_path): self.get_logger().error( - "Can't find database at supplied path " - + self._db_path - + ". If this is a new DB, create it as an empty directory." + f"Can't find database at supplied path {self._db_path}. If this is a new DB, create it as an empty " + f"directory." ) sys.exit(1) @@ -158,11 +158,13 @@ def block_mongo_kill(): if self.repl_set is not None: cmd.append("--replSet") cmd.append(self.repl_set) + + self.get_logger().info(f"Running command {' '.join(cmd)}") self._mongo_process = subprocess.Popen( cmd, stdout=subprocess.PIPE, preexec_fn=block_mongo_kill ) - while self._mongo_process.poll() is None: # and not rospy.is_shutdown(): + while self._mongo_process.poll() is None: # and rclpy.ok(): try: stdout = self._mongo_process.stdout.readline().decode("utf-8") except IOError as e: # probably interupt because shutdown cut it up @@ -176,13 +178,15 @@ def block_mongo_kill(): else: self.get_logger().info(stdout.strip()) - if stdout.find("waiting for connections on port") != -1: + if not self._ready and stdout.find("mongod startup complete") != -1: self._ready = True if self.repl_set is not None: try: self.initialize_repl_set() except Exception as e: - self.get_logger().warning("initialzing replSet failed: %s" % e) + self.get_logger().warning( + f"initialzing replSet failed: {e}" + ) if not rclpy.ok(): self.get_logger().error("MongoDB process stopped!") @@ -194,19 +198,24 @@ def block_mongo_kill(): self._gone_down = True - def _shutdown_srv_cb(self, req): - self.destroy_node() + def _shutdown_srv_cb( + self, request: Empty.Request, response: Empty.Response + ) -> Empty.Response: + # Calling shutdown exits the spin on the node. rclpy.shutdown() return Empty.Response() - def _wait_ready_srv_cb(self, req): + def _wait_ready_srv_cb( + self, request: Empty.Request, resp: Empty.Response + ) -> Empty.Response: while not self._ready: + self.get_logger().info("waiting") self.get_clock().sleep_for(Duration(seconds=0.1)) return Empty.Response() def initialize_repl_set(self): c = pymongo.Connection( - "%s:%d" % (self._mongo_host, self._mongo_port), slave_okay=True + f"{self._mongo_host}:{self._mongo_port}", slave_okay=True ) c.admin.command("replSetInitiate") c.close() @@ -216,9 +225,11 @@ def main(): rclpy.init() try: server = MongoServer() - rclpy.spin(server) + rclpy.spin(server, executor=MultiThreadedExecutor()) server.destroy_node() - rclpy.shutdown() + if rclpy.ok(): + # If the shutdown srv is called calling this again will cause a crash + rclpy.shutdown() finally: # TODO: The context on_shutdown doesn't seem to work, so moving that code here server.get_logger().info("Shutting down datacentre") From 47dd49cbfb0cb6c37250c17c879ccf60ee00ed96 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Tue, 15 Apr 2025 09:55:25 +0100 Subject: [PATCH 13/25] initial rough conversion of message store node --- .../scripts/message_store_node.py | 322 ++++++++++++------ 1 file changed, 225 insertions(+), 97 deletions(-) diff --git a/mongodb_store/mongodb_store/scripts/message_store_node.py b/mongodb_store/mongodb_store/scripts/message_store_node.py index f79209e..b8176fc 100755 --- a/mongodb_store/mongodb_store/scripts/message_store_node.py +++ b/mongodb_store/mongodb_store/scripts/message_store_node.py @@ -1,99 +1,176 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -from __future__ import absolute_import, print_function -from future.utils import iteritems """ Provides a service to store ROS message objects in a mongodb database in JSON. """ -import genpy -import rospy -import pymongo -from pymongo import GEO2D import json +from datetime import datetime, timezone + +import pymongo +from rcl_interfaces.srv import GetParameters +import rclpy from bson import json_util from bson.objectid import ObjectId -from datetime import * +from builtin_interfaces.msg import Time +from rcl_interfaces.msg import ParameterDescriptor, ParameterType +from rclpy.duration import Duration +from rclpy.executors import SingleThreadedExecutor, MultiThreadedExecutor from tf2_msgs.msg import TFMessage - -import mongodb_store_msgs.srv as dc_srv -import mongodb_store.util as dc_util -from mongodb_store_msgs.msg import StringPair, StringPairList, Insert +import mongodb_store.util as dc_util +from mongodb_store_msgs.msg import StringPair, StringPairList, Insert +from mongodb_store_msgs.srv import ( + MongoQueryMsg, + MongoUpdateMsg, + MongoDeleteMsg, + MongoInsertMsg, + MongoQuerywithProjectionMsg, +) MongoClient = dc_util.import_MongoClient() -class MessageStore(object): - def __init__(self, replicate_on_write=False): - use_daemon = rospy.get_param('mongodb_use_daemon', False) - connection_string = rospy.get_param('/mongodb_connection_string', '') +class MessageStore(rclpy.node.Node): + def __init__(self, replicate_on_write=False): + super().__init__("message_store") + use_daemon = self.declare_parameter( + "mongodb_use_daemon", + False, + descriptor=ParameterDescriptor(description="Use the daemon"), + ).value + connection_string = self.declare_parameter( + "mongodb_connection_string", + "", + descriptor=ParameterDescriptor(description=""), + ).value use_connection_string = len(connection_string) > 0 if use_connection_string: use_daemon = True - rospy.loginfo('Using connection string: %s', connection_string) + self.get_logger().info("Using connection string: %s", connection_string) # If you want to use a remote datacenter, then it should be set as false - use_localdatacenter = rospy.get_param('~mongodb_use_localdatacenter', True) - local_timeout = rospy.get_param('~local_timeout', 10) + use_localdatacenter = self.declare_parameter( + "mongodb_use_localdatacenter", True + ) + local_timeout = self.declare_parameter("local_timeout", 10).value if str(local_timeout).lower() == "none": local_timeout = None # wait for hostname and port for mongodb server - for _ in range(10): - if rospy.has_param('mongodb_host') and rospy.has_param('mongodb_port'): - break - rospy.sleep(1.0) - db_host = rospy.get_param('mongodb_host') - db_port = rospy.get_param('mongodb_port') + # TODO: This is limited to a specific name for the server + param_client = self.create_client( + GetParameters, "/mongodb_server/get_parameters" + ) + if not param_client.wait_for_service(10): + raise RuntimeError( + f"Could not find service {param_client.srv_name} from which to retrieve mongo host and port parameters" + ) + + request = GetParameters.Request() + request.names = ["mongodb_host", "mongodb_port"] + resp_future: rclpy.Future = param_client.call_async(request) + + # This is hacky but we need to do it in order to call this service before calling rclpy spin + rclpy.spin_until_future_complete( + self, + resp_future, + timeout_sec=5, + executor=SingleThreadedExecutor(), + ) + # Reset the executor, otherwise the node won't spin properly later + self.executor = None + host_value = resp_future.result().values[0] + port_value = resp_future.result().values[1] + if host_value.type != ParameterType.PARAMETER_STRING: + raise RuntimeError( + f"Parameter value for the mongodb_host param was not a string: {host_value}. Check the /mongodb_server parameters." + ) + if port_value.type != ParameterType.PARAMETER_INTEGER: + raise RuntimeError( + f"Parameter value for the mongodb_port param was not an integer: {port_value}. Check the /mongodb_server parameters." + ) + + mongodb_host = host_value.string_value + mongodb_port = port_value.integer_value + + db_host = self.declare_parameter( + "mongodb_host", mongodb_host, descriptor=ParameterDescriptor(description="") + ).value + db_port = self.declare_parameter( + "mongodb_port", mongodb_port, descriptor=ParameterDescriptor(description="") + ).value if use_daemon: if use_connection_string: - is_daemon_alive = dc_util.check_connection_to_mongod(None, None, connection_string=connection_string) + is_daemon_alive = dc_util.check_connection_to_mongod( + self, None, None, connection_string=connection_string + ) else: - is_daemon_alive = dc_util.check_connection_to_mongod(db_host, db_port) + is_daemon_alive = dc_util.check_connection_to_mongod( + self, db_host, db_port + ) if not is_daemon_alive: raise Exception("No Daemon?") elif use_localdatacenter: - rospy.loginfo('Waiting for local datacentre (timeout: %s)' % str(local_timeout)) - have_dc = dc_util.wait_for_mongo(local_timeout) + self.get_logger().info( + f"Waiting for local datacentre (timeout: {local_timeout})" + ) + have_dc = dc_util.wait_for_mongo(self, local_timeout) if not have_dc: raise Exception("No Datacentre?") - self.keep_trash = rospy.get_param('mongodb_keep_trash', True) + self.keep_trash = self.declare_parameter( + "mongodb_keep_trash", True, descriptor=ParameterDescriptor(description="") + ).value if use_connection_string: - self._mongo_client=MongoClient(connection_string) + self._mongo_client = MongoClient(connection_string) else: - self._mongo_client=MongoClient(db_host, db_port) + self._mongo_client = MongoClient(db_host, db_port) - self.replicate_on_write = rospy.get_param( - "mongodb_replicate_on_write", replicate_on_write) + self.replicate_on_write = self.declare_parameter( + "mongodb_replicate_on_write", replicate_on_write + ).value if self.replicate_on_write: - rospy.logwarn( + self.get_logger().warning( "The option 'replicate_on_write' is now deprecated and will be removed. " "Use 'Replication' on MongoDB instead: " - "https://docs.mongodb.com/manual/replication/") - - extras = rospy.get_param('mongodb_store_extras', []) + "https://docs.mongodb.com/manual/replication/" + ) + + extras = self.declare_parameter( + "mongodb_store_extras", + [], + descriptor=ParameterDescriptor(description=""), + ).value self.extra_clients = [] for extra in extras: try: self.extra_clients.append(MongoClient(extra[0], extra[1])) except pymongo.errors.ConnectionFailure as e: - rospy.logwarn('Could not connect to extra datacentre at %s:%s' % (extra[0], extra[1])) - rospy.loginfo('Replicating content to a futher %s datacentres',len(self.extra_clients)) + self.get_logger().warning( + f"Could not connect to extra datacentre at {extra[0]}:{extra[1]}" + ) + self.get_logger().info( + f"Replicating content to a futher {len(self.extra_clients)} datacentres" + ) # advertise ros services for attr in dir(self): if attr.endswith("_ros_srv"): - service=getattr(self, attr) - rospy.Service("/message_store/"+attr[:-8], service.type, service) - - self.queue_size = rospy.get_param("queue_size", 100) - self.sub_insert = rospy.Subscriber("/message_store/insert", Insert, - self.insert_ros_msg, - queue_size=self.queue_size) + service = getattr(self, attr) + self.create_service( + service.type, "/message_store/" + attr[:-8], service + ) + + self.queue_size = self.declare_parameter( + "queue_size", 100, descriptor=ParameterDescriptor(description="") + ).value + self.sub_insert = self.create_subscription( + Insert, + "/message_store/insert", + self.insert_ros_msg, + self.queue_size, + ) def insert_ros_msg(self, msg): """ @@ -113,12 +190,12 @@ def insert_ros_srv(self, req): # get requested collection from the db, creating if necessary collection = self._mongo_client[req.database][req.collection] # check if the object has the location attribute - if hasattr(obj, 'pose'): + if hasattr(obj, "pose"): # if it does create a location index collection.create_index([("loc", pymongo.GEO2D)]) - #check if the object has the location attribute - if hasattr(obj, 'geotype'): + # check if the object has the location attribute + if hasattr(obj, "geotype"): # if it does create a location index collection.create_index([("geoloc", pymongo.GEOSPHERE)]) @@ -128,20 +205,24 @@ def insert_ros_srv(self, req): # collection.create_index([("datetime", pymongo.GEO2D)]) # try: - stamp = rospy.get_rostime() - meta['inserted_at'] = datetime.utcfromtimestamp(stamp.to_sec()) - meta['inserted_by'] = req._connection_header['callerid'] - if hasattr(obj, "header") and hasattr(obj.header, "stamp") and\ - isinstance(obj.header.stamp, genpy.Time): + stamp = self.get_clock().now().to_msg() + meta["inserted_at"] = datetime.fromtimestamp(stamp.to_sec(), timezone.UTC) + meta["inserted_by"] = req._connection_header["callerid"] + if ( + hasattr(obj, "header") + and hasattr(obj.header, "stamp") + and isinstance(obj.header.stamp, Time) + ): stamp = obj.header.stamp elif isinstance(obj, TFMessage): if obj.transforms: - transforms = sorted(obj.transforms, - key=lambda m: m.header.stamp, reverse=True) + transforms = sorted( + obj.transforms, key=lambda m: m.header.stamp, reverse=True + ) stamp = transforms[0].header.stamp - meta['published_at'] = datetime.utcfromtimestamp(stamp.to_sec()) - meta['timestamp'] = stamp.to_nsec() + meta["published_at"] = datetime.fromtimestamp(stamp.to_sec(), timezone.utc) + meta["timestamp"] = stamp.to_nsec() obj_id = dc_util.store_message(collection, obj, meta) @@ -153,9 +234,9 @@ def insert_ros_srv(self, req): return str(obj_id) # except Exception, e: - # print e + # print e - insert_ros_srv.type=dc_srv.MongoInsertMsg + insert_ros_srv.type = MongoInsertMsg def delete_ros_srv(self, req): """ @@ -163,7 +244,9 @@ def delete_ros_srv(self, req): """ # Get the message collection = self._mongo_client[req.database][req.collection] - docs = dc_util.query_message(collection, {"_id": ObjectId(req.document_id)}, find_one=True) + docs = dc_util.query_message( + collection, {"_id": ObjectId(req.document_id)}, find_one=True + ) if len(docs) != 1: return False @@ -177,18 +260,19 @@ def delete_ros_srv(self, req): bk_collection = self._mongo_client[req.database][req.collection + "_Trash"] bk_collection.save(message) - # also repeat in extras if self.replicate_on_write: for extra_client in self.extra_clients: extra_collection = extra_client[req.database][req.collection] extra_collection.remove({"_id": ObjectId(req.document_id)}) - extra_bk_collection = extra_client[req.database][req.collection + "_Trash"] + extra_bk_collection = extra_client[req.database][ + req.collection + "_Trash" + ] extra_bk_collection.save(message) return True - delete_ros_srv.type=dc_srv.MongoDeleteMsg + delete_ros_srv.type = MongoDeleteMsg def update_ros_srv(self, req): """ @@ -205,25 +289,32 @@ def update_ros_srv(self, req): # TODO start using some string constants! - rospy.logdebug("update spec document: %s", obj_query) + self.get_logger().debug(f"update spec document: {obj_query}") # deserialize data into object obj = dc_util.deserialise_message(req.message) meta = dc_util.string_pair_list_to_dictionary(req.meta) - meta['last_updated_at'] = datetime.utcfromtimestamp(rospy.get_rostime().to_sec()) - meta['last_updated_by'] = req._connection_header['callerid'] + meta["last_updated_at"] = datetime.fromtimestamp( + self.get_clock().now().seconds_nanoseconds()[0], timezone.utc + ) + meta["last_updated_by"] = req._connection_header["callerid"] - (obj_id, altered) = dc_util.update_message(collection, obj_query, obj, meta, req.upsert) + (obj_id, altered) = dc_util.update_message( + collection, obj_query, obj, meta, req.upsert + ) if self.replicate_on_write: # also do update to extra datacentres for extra_client in self.extra_clients: extra_collection = extra_client[req.database][req.collection] - dc_util.update_message(extra_collection, obj_query, obj, meta, req.upsert) + dc_util.update_message( + extra_collection, obj_query, obj, meta, req.upsert + ) return str(obj_id), altered - update_ros_srv.type=dc_srv.MongoUpdateMsg + + update_ros_srv.type = MongoUpdateMsg def to_query_dict(self, message_query, meta_query): """ @@ -231,7 +322,7 @@ def to_query_dict(self, message_query, meta_query): """ obj_query = dc_util.string_pair_list_to_dictionary(message_query) bare_meta_query = dc_util.string_pair_list_to_dictionary(meta_query) - for (k, v) in iteritems(bare_meta_query): + for k, v in bare_meta_query.items(): obj_query["_meta." + k] = v return obj_query @@ -249,42 +340,66 @@ def query_messages_ros_srv(self, req): # TODO start using some string constants! - rospy.logdebug("query document: %s", obj_query) + self.get_logger().debug(f"query document: {obj_query}") # this is a list of entries in dict format including meta sort_query_dict = dc_util.string_pair_list_to_dictionary(req.sort_query) sort_query_tuples = [] - for k, v in iteritems(sort_query_dict): + for k, v in sort_query_dict.items(): try: sort_query_tuples.append((k, int(v))) except ValueError: - sort_query_tuples.append((k,v)) - # this is a list of entries in dict format including meta - + sort_query_tuples.append((k, v)) + # this is a list of entries in dict format including meta - projection_query_dict = dc_util.string_pair_list_to_dictionary(req.projection_query) - projection_meta_dict = dict() + projection_query_dict = dc_util.string_pair_list_to_dictionary( + req.projection_query + ) + projection_meta_dict = dict() projection_meta_dict["_meta"] = 1 - entries = dc_util.query_message( - collection, obj_query, sort_query_tuples, projection_query_dict, req.single, req.limit) + entries = dc_util.query_message( + collection, + obj_query, + sort_query_tuples, + projection_query_dict, + req.single, + req.limit, + ) if projection_query_dict: meta_entries = dc_util.query_message( - collection, obj_query, sort_query_tuples, projection_meta_dict, req.single, req.limit) - + collection, + obj_query, + sort_query_tuples, + projection_meta_dict, + req.single, + req.limit, + ) # keep trying clients until we find an answer if self.replicate_on_write: for extra_client in self.extra_clients: if len(entries) == 0: extra_collection = extra_client[req.database][req.collection] - entries = dc_util.query_message( - extra_collection, obj_query, sort_query_tuples, projection_query_dict, req.single, req.limit) + entries = dc_util.query_message( + extra_collection, + obj_query, + sort_query_tuples, + projection_query_dict, + req.single, + req.limit, + ) if projection_query_dict: meta_entries = dc_util.query_message( - extra_collection, obj_query, sort_query_tuples, projection_meta_dict, req.single, req.limit) + extra_collection, + obj_query, + sort_query_tuples, + projection_meta_dict, + req.single, + req.limit, + ) if len(entries) > 0: - rospy.loginfo("found result in extra datacentre") + self.get_logger().info("found result in extra datacentre") else: break @@ -299,18 +414,31 @@ def query_messages_ros_srv(self, req): # instantiate the ROS message object from the dictionary retrieved from the db message = dc_util.dictionary_to_message(entry, cls) # the serialise this object in order to be sent in a generic form - serialised_messages = serialised_messages + (dc_util.serialise_message(message), ) + serialised_messages = serialised_messages + ( + dc_util.serialise_message(message), + ) # add ObjectID into meta as it might be useful later if projection_query_dict: entry["_meta"]["_id"] = meta_entries[idx]["_id"] else: entry["_meta"]["_id"] = entry["_id"] # serialise meta - metas = metas + (StringPairList([StringPair(dc_srv.MongoQueryMsgRequest.JSON_QUERY, json.dumps(entry["_meta"], default=json_util.default))]), ) + metas = metas + ( + StringPairList( + pairs=[ + StringPair( + first=MongoQueryMsg.Request.JSON_QUERY, + second=json.dumps( + entry["_meta"], default=json_util.default + ), + ) + ] + ), + ) return [serialised_messages, metas] - query_messages_ros_srv.type=dc_srv.MongoQueryMsg + query_messages_ros_srv.type = MongoQueryMsg def query_with_projection_messages_ros_srv(self, req): """ @@ -318,12 +446,12 @@ def query_with_projection_messages_ros_srv(self, req): """ return self.query_messages_ros_srv(req) - query_with_projection_messages_ros_srv.type=dc_srv.MongoQuerywithProjectionMsg - + query_with_projection_messages_ros_srv.type = MongoQuerywithProjectionMsg -if __name__ == '__main__': - rospy.init_node("message_store") +def main(): + rclpy.init() store = MessageStore() - - rospy.spin() + rclpy.spin(store, executor=MultiThreadedExecutor()) + store.destroy_node() + rclpy.shutdown() From a257ab7684355ecdd254cd9f0d7e2a172a3a888d Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Tue, 15 Apr 2025 09:56:20 +0100 Subject: [PATCH 14/25] move message store node out of scripts --- mongodb_store/mongodb_store/{scripts => }/message_store_node.py | 0 mongodb_store/setup.py | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) rename mongodb_store/mongodb_store/{scripts => }/message_store_node.py (100%) diff --git a/mongodb_store/mongodb_store/scripts/message_store_node.py b/mongodb_store/mongodb_store/message_store_node.py similarity index 100% rename from mongodb_store/mongodb_store/scripts/message_store_node.py rename to mongodb_store/mongodb_store/message_store_node.py diff --git a/mongodb_store/setup.py b/mongodb_store/setup.py index 8f88b39..20c6e56 100644 --- a/mongodb_store/setup.py +++ b/mongodb_store/setup.py @@ -51,7 +51,7 @@ def glob_files( "config_manager = mongodb_store.scripts.config_manager:main", "example_message_store_client = mongodb_store.scripts.example_message_store_client:main", "example_multi_event_log = mongodb_store.scripts.example_multi_event_log:main", - "message_store_node = mongodb_store.scripts.message_store_node:main", + "message_store_node = mongodb_store.message_store_node:main", "mongo_bridge = mongodb_store.scripts.mongo_bridge:main", "mongodb_play = mongodb_store.scripts.mongodb_play:main", "mongodb_server = mongodb_store.scripts.mongodb_server:main", From 3bdcbc1ed11c7af316647091c762b81d363cb8a1 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Tue, 15 Apr 2025 10:50:03 +0100 Subject: [PATCH 15/25] use new util function to wait/call services before rclpy spin, remove replication --- .../mongodb_store/message_store_node.py | 102 +++--------------- mongodb_store/mongodb_store/util.py | 91 ++++++++++++---- 2 files changed, 87 insertions(+), 106 deletions(-) diff --git a/mongodb_store/mongodb_store/message_store_node.py b/mongodb_store/mongodb_store/message_store_node.py index b8176fc..f96ed17 100755 --- a/mongodb_store/mongodb_store/message_store_node.py +++ b/mongodb_store/mongodb_store/message_store_node.py @@ -6,14 +6,13 @@ from datetime import datetime, timezone import pymongo -from rcl_interfaces.srv import GetParameters import rclpy from bson import json_util from bson.objectid import ObjectId from builtin_interfaces.msg import Time from rcl_interfaces.msg import ParameterDescriptor, ParameterType -from rclpy.duration import Duration -from rclpy.executors import SingleThreadedExecutor, MultiThreadedExecutor +from rcl_interfaces.srv import GetParameters +from rclpy.executors import MultiThreadedExecutor from tf2_msgs.msg import TFMessage import mongodb_store.util as dc_util @@ -60,26 +59,19 @@ def __init__(self, replicate_on_write=False): param_client = self.create_client( GetParameters, "/mongodb_server/get_parameters" ) - if not param_client.wait_for_service(10): + request = GetParameters.Request() + request.names = ["mongodb_host", "mongodb_port"] + + result, message = dc_util.check_and_get_service_result_async( + self, param_client, request, existence_timeout=10 + ) + if result is None: raise RuntimeError( f"Could not find service {param_client.srv_name} from which to retrieve mongo host and port parameters" ) - request = GetParameters.Request() - request.names = ["mongodb_host", "mongodb_port"] - resp_future: rclpy.Future = param_client.call_async(request) - - # This is hacky but we need to do it in order to call this service before calling rclpy spin - rclpy.spin_until_future_complete( - self, - resp_future, - timeout_sec=5, - executor=SingleThreadedExecutor(), - ) - # Reset the executor, otherwise the node won't spin properly later - self.executor = None - host_value = resp_future.result().values[0] - port_value = resp_future.result().values[1] + host_value = result.values[0] + port_value = result.values[1] if host_value.type != ParameterType.PARAMETER_STRING: raise RuntimeError( f"Parameter value for the mongodb_host param was not a string: {host_value}. Check the /mongodb_server parameters." @@ -117,6 +109,8 @@ def __init__(self, replicate_on_write=False): have_dc = dc_util.wait_for_mongo(self, local_timeout) if not have_dc: raise Exception("No Datacentre?") + else: + self.get_logger().info("Got datacentre") self.keep_trash = self.declare_parameter( "mongodb_keep_trash", True, descriptor=ParameterDescriptor(description="") @@ -132,28 +126,11 @@ def __init__(self, replicate_on_write=False): ).value if self.replicate_on_write: self.get_logger().warning( - "The option 'replicate_on_write' is now deprecated and will be removed. " + "The option 'replicate_on_write' is now deprecated and will not function. " "Use 'Replication' on MongoDB instead: " "https://docs.mongodb.com/manual/replication/" ) - extras = self.declare_parameter( - "mongodb_store_extras", - [], - descriptor=ParameterDescriptor(description=""), - ).value - self.extra_clients = [] - for extra in extras: - try: - self.extra_clients.append(MongoClient(extra[0], extra[1])) - except pymongo.errors.ConnectionFailure as e: - self.get_logger().warning( - f"Could not connect to extra datacentre at {extra[0]}:{extra[1]}" - ) - self.get_logger().info( - f"Replicating content to a futher {len(self.extra_clients)} datacentres" - ) - # advertise ros services for attr in dir(self): if attr.endswith("_ros_srv"): @@ -226,12 +203,6 @@ def insert_ros_srv(self, req): obj_id = dc_util.store_message(collection, obj, meta) - if self.replicate_on_write: - # also do insert to extra datacentres, making sure object ids are consistent - for extra_client in self.extra_clients: - extra_collection = extra_client[req.database][req.collection] - dc_util.store_message(extra_collection, obj, meta, obj_id) - return str(obj_id) # except Exception, e: # print e @@ -260,16 +231,6 @@ def delete_ros_srv(self, req): bk_collection = self._mongo_client[req.database][req.collection + "_Trash"] bk_collection.save(message) - # also repeat in extras - if self.replicate_on_write: - for extra_client in self.extra_clients: - extra_collection = extra_client[req.database][req.collection] - extra_collection.remove({"_id": ObjectId(req.document_id)}) - extra_bk_collection = extra_client[req.database][ - req.collection + "_Trash" - ] - extra_bk_collection.save(message) - return True delete_ros_srv.type = MongoDeleteMsg @@ -304,14 +265,6 @@ def update_ros_srv(self, req): collection, obj_query, obj, meta, req.upsert ) - if self.replicate_on_write: - # also do update to extra datacentres - for extra_client in self.extra_clients: - extra_collection = extra_client[req.database][req.collection] - dc_util.update_message( - extra_collection, obj_query, obj, meta, req.upsert - ) - return str(obj_id), altered update_ros_srv.type = MongoUpdateMsg @@ -376,33 +329,6 @@ def query_messages_ros_srv(self, req): req.limit, ) - # keep trying clients until we find an answer - if self.replicate_on_write: - for extra_client in self.extra_clients: - if len(entries) == 0: - extra_collection = extra_client[req.database][req.collection] - entries = dc_util.query_message( - extra_collection, - obj_query, - sort_query_tuples, - projection_query_dict, - req.single, - req.limit, - ) - if projection_query_dict: - meta_entries = dc_util.query_message( - extra_collection, - obj_query, - sort_query_tuples, - projection_meta_dict, - req.single, - req.limit, - ) - if len(entries) > 0: - self.get_logger().info("found result in extra datacentre") - else: - break - serialised_messages = () metas = () diff --git a/mongodb_store/mongodb_store/util.py b/mongodb_store/mongodb_store/util.py index 7a42052..e6e3ae3 100644 --- a/mongodb_store/mongodb_store/util.py +++ b/mongodb_store/mongodb_store/util.py @@ -1,14 +1,18 @@ -import rclpy -import rclpy.type_support -import rclpy.action import importlib import json +import typing from datetime import datetime from datetime import timezone from io import BytesIO as Buffer +import rclpy +import rclpy.node +import rclpy.client +import rclpy.type_support from bson import json_util, Binary from pymongo.errors import ConnectionFailure +from rclpy.callback_groups import MutuallyExclusiveCallbackGroup +from rclpy.executors import MultiThreadedExecutor from std_srvs.srv import Empty from mongodb_store_msgs.msg import SerialisedMessage @@ -56,20 +60,72 @@ def wait_for_mongo(parent_node: rclpy.node.Node, timeout=60, ns="/datacentre"): :Returns: | bool : True on success, False if server not even started. """ - # Check that mongo is live, create connection + # # Check that mongo is live, create connection service = ns + "/wait_ready" - client = parent_node.create_client(Empty, service) - - valid = client.wait_for_service(timeout) - if not valid: + wait_client = parent_node.create_client( + Empty, service + ) + + result, message = check_and_get_service_result_async( + parent_node, wait_client, Empty.Request() + ) + if result is None: parent_node.get_logger().error( "Can't connect to MongoDB server. Make sure mongodb_store/mongodb_server.py node is started." ) return False - client.call(Empty.Request()) return True +def check_and_get_service_result_async( + node: rclpy.node.Node, + client: rclpy.client.Client, + request, + existence_timeout: typing.Optional[float] = 1, + spin_timeout: typing.Optional[float] = None, +) -> typing.Tuple[typing.Optional[typing.Any], str]: + """ + Check the service for the given client exists, then call it and retrieve the response asynchronously, + but block to do so. Calls the executor from the node associated with the client, using its + spin_until_future_complete function + + Note: If the client is being called from a callback (i.e. subscriber callback, service callback, any actionserver + callback) You must ensure that the client is in a separate callback group to the one which is initiating this + call. If it is not, you will probably get a silent deadlock. + + Args: + node: Node which created the client. TODO: Is this needed? The client has a context and handle but not sure if those have references to the node + client: Client to call + request: Request to send to the client + existence_timeout: How long to wait for the service to become available + spin_timeout: How long to spin waiting for the result before timing out + + Returns: + Tuple with result of the call, or None if it failed, and a message + """ + if not client.wait_for_service(timeout_sec=existence_timeout): + message = f"Couldn't find {client.srv_name}" + node.get_logger().error(message) + return None, message + resp_future = client.call_async(request) + # Don't use rclpy.spin_until_future_completes on the node because then the node is removed from the global + # executor and will not receive any further callbacks + if not node.executor: + # do this in case the node doesn't have an executor, and remove the executor after we're done, otherwise the + # executor is permanently set as that node's executor + rclpy.spin_until_future_complete( + node, + resp_future, + timeout_sec=spin_timeout, + executor=MultiThreadedExecutor(), + ) + node.executor = None + else: + node.executor.spin_until_future_complete(resp_future, timeout_sec=spin_timeout) + + return resp_future.result(), f"Successfully called {client.srv_name}" + + def check_for_pymongo(): """ Checks for required version of pymongo python library. @@ -81,12 +137,8 @@ def check_for_pymongo(): import pymongo except: print("ERROR!!!") - print( - "Can't import pymongo, this is needed by mongodb_store." - ) - print( - "Make sure it is installed (pip install pymongo)" - ) + print("Can't import pymongo, this is needed by mongodb_store.") + print("Make sure it is installed (pip install pymongo)") return False return True @@ -132,8 +184,6 @@ def document_to_msg_and_meta(document, TYPE): return meta, msg - - def document_to_msg(document, TYPE): """ Given a document return ROS message @@ -360,7 +410,12 @@ def dictionary_to_message(dictionary, cls): def query_message( - collection, query_doc, sort_query=None, projection_query=None, find_one=False, limit=0 + collection, + query_doc, + sort_query=None, + projection_query=None, + find_one=False, + limit=0, ): """ Peform a query for a stored messages, returning results in list. From c67c214be37a8ff61ea981a7bbb542cb958ab82e Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Tue, 15 Apr 2025 13:45:12 +0100 Subject: [PATCH 16/25] message store insertion uses async service call, make query tuples string pair lists --- mongodb_store/mongodb_store/message_store.py | 56 +++++++++++--------- 1 file changed, 32 insertions(+), 24 deletions(-) diff --git a/mongodb_store/mongodb_store/message_store.py b/mongodb_store/mongodb_store/message_store.py index 3add96b..ab58aa6 100644 --- a/mongodb_store/mongodb_store/message_store.py +++ b/mongodb_store/mongodb_store/message_store.py @@ -140,7 +140,9 @@ def insert( request.message = serialised_msg if wait: - return self.insert_srv.call(request).id + return dc_util.check_and_get_service_result_async( + self.parent_node, self.insert_srv, request + )[0].id else: msg = Insert( database=self.database, @@ -360,31 +362,37 @@ def query( # serialise the json queries to strings using json_util.dumps - message_tuple = ( - StringPair( - first=MongoQueryMsg.Request.JSON_QUERY, - second=json.dumps(message_query, default=json_util.default), - ), + message_tuple = StringPairList( + pairs=[ + StringPair( + first=MongoQueryMsg.Request.JSON_QUERY, + second=json.dumps(message_query, default=json_util.default), + ), + ] ) - meta_tuple = ( - StringPair( - first=MongoQueryMsg.Request.JSON_QUERY, - second=json.dumps(meta_query, default=json_util.default), - ), + meta_tuple = StringPairList( + pairs=[ + StringPair( + first=MongoQueryMsg.Request.JSON_QUERY, + second=json.dumps(meta_query, default=json_util.default), + ), + ] ) - projection_tuple = ( - StringPair( - first=MongoQueryMsg.Request.JSON_QUERY, - second=json.dumps(projection_query, default=json_util.default), - ), + projection_tuple = StringPairList( + pairs=[ + StringPair( + first=MongoQueryMsg.Request.JSON_QUERY, + second=json.dumps(projection_query, default=json_util.default), + ), + ] ) if len(sort_query) > 0: - sort_tuple = [ - StringPair(first=str(k), second=str(v)) for k, v in sort_query - ] + sort_tuple = StringPairList( + pairs=[StringPair(first=str(k), second=str(v)) for k, v in sort_query] + ) else: - sort_tuple = [] + sort_tuple = StringPairList() request = MongoQueryMsg.Request() request.database = self.database @@ -392,10 +400,10 @@ def query( request.type = type request.single = single request.limit = limit - request.message_query = message_query - request.meta_query = meta_query - request.projection_query = projection_query - request.sort_query = sort_query + request.message_query = message_tuple + request.meta_query = meta_tuple + request.projection_query = projection_tuple + request.sort_query = sort_tuple response = self.query_srv.call(request) From 0d98f7fc97df49c938680d704fef6b3e46c1dedf Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Tue, 15 Apr 2025 13:47:07 +0100 Subject: [PATCH 17/25] update service callbacks to ros2 (insertion works), serialisation now works - insertion service is functional, had to fix timestamps to convert from ros2 clock. No longer stores the callerid because I'm not sure its accessible - serialisation updated to ros2 - delete references to soma in util - function to get namespaced type from message as it's no longer accessible at msg._type - fix json loading null returning None rather than empty dict --- .../mongodb_store/message_store_node.py | 142 ++++++----- mongodb_store/mongodb_store/util.py | 234 ++++-------------- 2 files changed, 129 insertions(+), 247 deletions(-) diff --git a/mongodb_store/mongodb_store/message_store_node.py b/mongodb_store/mongodb_store/message_store_node.py index f96ed17..569f087 100755 --- a/mongodb_store/mongodb_store/message_store_node.py +++ b/mongodb_store/mongodb_store/message_store_node.py @@ -2,6 +2,8 @@ Provides a service to store ROS message objects in a mongodb database in JSON. """ +import rosidl_runtime_py +import rosidl_runtime_py.utilities import json from datetime import datetime, timezone @@ -154,18 +156,20 @@ def insert_ros_msg(self, msg): Receives a message published """ # actually procedure is the same - self.insert_ros_srv(msg) + self.insert_ros_srv(msg, MongoInsertMsg.Response()) - def insert_ros_srv(self, req): + def insert_ros_srv( + self, request: MongoInsertMsg.Request, response: MongoInsertMsg.Response + ) -> MongoInsertMsg.Response: """ Receives a """ # deserialize data into object - obj = dc_util.deserialise_message(req.message) + obj = dc_util.deserialise_message(request.message) # convert input tuple to dict - meta = dc_util.string_pair_list_to_dictionary(req.meta) + meta = dc_util.string_pair_list_to_dictionary(request.meta) # get requested collection from the db, creating if necessary - collection = self._mongo_client[req.database][req.collection] + collection = self._mongo_client[request.database][request.collection] # check if the object has the location attribute if hasattr(obj, "pose"): # if it does create a location index @@ -181,91 +185,105 @@ def insert_ros_srv(self, req): # if it does create a location index # collection.create_index([("datetime", pymongo.GEO2D)]) - # try: - stamp = self.get_clock().now().to_msg() - meta["inserted_at"] = datetime.fromtimestamp(stamp.to_sec(), timezone.UTC) - meta["inserted_by"] = req._connection_header["callerid"] - if ( - hasattr(obj, "header") - and hasattr(obj.header, "stamp") - and isinstance(obj.header.stamp, Time) - ): - stamp = obj.header.stamp - elif isinstance(obj, TFMessage): - if obj.transforms: - transforms = sorted( - obj.transforms, key=lambda m: m.header.stamp, reverse=True - ) - stamp = transforms[0].header.stamp - - meta["published_at"] = datetime.fromtimestamp(stamp.to_sec(), timezone.utc) - meta["timestamp"] = stamp.to_nsec() - - obj_id = dc_util.store_message(collection, obj, meta) - - return str(obj_id) - # except Exception, e: - # print e + try: + stamp = self.get_clock().now() + sec_ns = stamp.seconds_nanoseconds() + fl = float(f"{sec_ns[0]}.{sec_ns[1]}") + meta["inserted_at"] = datetime.fromtimestamp(fl, timezone.utc) + # TODO: Retrieving this information seems to be much harder/impossible in ros2 + # meta["inserted_by"] = request._connection_header["callerid"] + + if ( + hasattr(obj, "header") + and hasattr(obj.header, "stamp") + and isinstance(obj.header.stamp, Time) + ): + stamp = obj.header.stamp + elif isinstance(obj, TFMessage): + if obj.transforms: + transforms = sorted( + obj.transforms, key=lambda m: m.header.stamp, reverse=True + ) + stamp = transforms[0].header.stamp + + sec_ns = stamp.seconds_nanoseconds() + fl = float(f"{sec_ns[0]}.{sec_ns[1]}") + meta["published_at"] = datetime.fromtimestamp(fl, timezone.utc) + meta["timestamp"] = stamp.nanoseconds + + obj_id = dc_util.store_message(collection, obj, meta) + return MongoInsertMsg.Response(id=str(obj_id)) + except Exception as e: + import traceback + + print(traceback.format_exc()) + return MongoInsertMsg.Response(id="") insert_ros_srv.type = MongoInsertMsg - def delete_ros_srv(self, req): + def delete_ros_srv( + self, request: MongoDeleteMsg.Request, response: MongoDeleteMsg.Response + ) -> MongoDeleteMsg.Response: """ Deletes a message by ID """ # Get the message - collection = self._mongo_client[req.database][req.collection] + collection = self._mongo_client[request.database][request.collection] docs = dc_util.query_message( - collection, {"_id": ObjectId(req.document_id)}, find_one=True + collection, {"_id": ObjectId(request.document_id)}, find_one=True ) if len(docs) != 1: - return False + return MongoDeleteMsg.Response(success=False) message = docs[0] # Remove the doc - collection.remove({"_id": ObjectId(req.document_id)}) + collection.remove({"_id": ObjectId(request.document_id)}) if self.keep_trash: # But keep it into "trash" - bk_collection = self._mongo_client[req.database][req.collection + "_Trash"] + bk_collection = self._mongo_client[request.database][ + request.collection + "_Trash" + ] bk_collection.save(message) - return True + return MongoDeleteMsg.Response(success=True) delete_ros_srv.type = MongoDeleteMsg - def update_ros_srv(self, req): + def update_ros_srv( + self, request: MongoUpdateMsg.Request, response: MongoUpdateMsg.Response + ) -> MongoUpdateMsg.Response: """ Updates a msg in the store """ # rospy.lrosoginfo("called") - collection = self._mongo_client[req.database][req.collection] + collection = self._mongo_client[request.database][request.collection] # build the query doc - obj_query = self.to_query_dict(req.message_query, req.meta_query) + obj_query = self.to_query_dict(request.message_query, request.meta_query) # restrict results to have the type asked for - obj_query["_meta.stored_type"] = req.message.type + obj_query["_meta.stored_type"] = request.message.type # TODO start using some string constants! self.get_logger().debug(f"update spec document: {obj_query}") # deserialize data into object - obj = dc_util.deserialise_message(req.message) + obj = dc_util.deserialise_message(request.message) - meta = dc_util.string_pair_list_to_dictionary(req.meta) + meta = dc_util.string_pair_list_to_dictionary(request.meta) meta["last_updated_at"] = datetime.fromtimestamp( self.get_clock().now().seconds_nanoseconds()[0], timezone.utc ) - meta["last_updated_by"] = req._connection_header["callerid"] + meta["last_updated_by"] = request._connection_header["callerid"] (obj_id, altered) = dc_util.update_message( - collection, obj_query, obj, meta, req.upsert + collection, obj_query, obj, meta, request.upsert ) - return str(obj_id), altered + return MongoUpdateMsg.Response(id=str(obj_id), success=altered) update_ros_srv.type = MongoUpdateMsg @@ -279,24 +297,26 @@ def to_query_dict(self, message_query, meta_query): obj_query["_meta." + k] = v return obj_query - def query_messages_ros_srv(self, req): + def query_messages_ros_srv( + self, request: MongoQueryMsg.Request, response: MongoQueryMsg.Response + ) -> MongoQueryMsg.Response: """ Returns t """ - collection = self._mongo_client[req.database][req.collection] + collection = self._mongo_client[request.database][request.collection] # build the query doc - obj_query = self.to_query_dict(req.message_query, req.meta_query) + obj_query = self.to_query_dict(request.message_query, request.meta_query) # restrict results to have the type asked for - obj_query["_meta.stored_type"] = req.type + obj_query["_meta.stored_type"] = request.type # TODO start using some string constants! self.get_logger().debug(f"query document: {obj_query}") # this is a list of entries in dict format including meta - sort_query_dict = dc_util.string_pair_list_to_dictionary(req.sort_query) + sort_query_dict = dc_util.string_pair_list_to_dictionary(request.sort_query) sort_query_tuples = [] for k, v in sort_query_dict.items(): try: @@ -306,7 +326,7 @@ def query_messages_ros_srv(self, req): # this is a list of entries in dict format including meta projection_query_dict = dc_util.string_pair_list_to_dictionary( - req.projection_query + request.projection_query ) projection_meta_dict = dict() projection_meta_dict["_meta"] = 1 @@ -316,8 +336,8 @@ def query_messages_ros_srv(self, req): obj_query, sort_query_tuples, projection_query_dict, - req.single, - req.limit, + request.single, + request.limit, ) if projection_query_dict: meta_entries = dc_util.query_message( @@ -325,8 +345,8 @@ def query_messages_ros_srv(self, req): obj_query, sort_query_tuples, projection_meta_dict, - req.single, - req.limit, + request.single, + request.limit, ) serialised_messages = () @@ -336,9 +356,11 @@ def query_messages_ros_srv(self, req): # load the class object for this type # TODO this should be the same for every item in the list, so could reuse - cls = dc_util.load_class(entry["_meta"]["stored_class"]) + cls = rosidl_runtime_py.utilities.get_interface( + entry["_meta"]["stored_class"] + ) # instantiate the ROS message object from the dictionary retrieved from the db - message = dc_util.dictionary_to_message(entry, cls) + message = rosidl_runtime_py.set_message_fields(cls(), entry) # the serialise this object in order to be sent in a generic form serialised_messages = serialised_messages + ( dc_util.serialise_message(message), @@ -362,7 +384,7 @@ def query_messages_ros_srv(self, req): ), ) - return [serialised_messages, metas] + return MongoQueryMsg.Response(messages=serialised_messages, metas=metas) query_messages_ros_srv.type = MongoQueryMsg @@ -370,7 +392,7 @@ def query_with_projection_messages_ros_srv(self, req): """ Returns t """ - return self.query_messages_ros_srv(req) + return self.query_messages_ros_srv(req, MongoQueryMsg.Response()) query_with_projection_messages_ros_srv.type = MongoQuerywithProjectionMsg diff --git a/mongodb_store/mongodb_store/util.py b/mongodb_store/mongodb_store/util.py index e6e3ae3..b0b6470 100644 --- a/mongodb_store/mongodb_store/util.py +++ b/mongodb_store/mongodb_store/util.py @@ -1,5 +1,8 @@ import importlib import json + +import pymongo.collection +import yaml import typing from datetime import datetime from datetime import timezone @@ -9,6 +12,8 @@ import rclpy.node import rclpy.client import rclpy.type_support +import rclpy.serialization +import rosidl_runtime_py.utilities from bson import json_util, Binary from pymongo.errors import ConnectionFailure from rclpy.callback_groups import MutuallyExclusiveCallbackGroup @@ -62,9 +67,7 @@ def wait_for_mongo(parent_node: rclpy.node.Node, timeout=60, ns="/datacentre"): """ # # Check that mongo is live, create connection service = ns + "/wait_ready" - wait_client = parent_node.create_client( - Empty, service - ) + wait_client = parent_node.create_client(Empty, service) result, message = check_and_get_service_result_async( parent_node, wait_client, Empty.Request() @@ -165,6 +168,7 @@ def mongo_client_wrapper(*args, **kwargs): def _fill_msg(msg, dic): """ + TODO: Remove? Given a ROS msg and a dictionary of the right values, fill in the msg """ for i in dic: @@ -176,6 +180,7 @@ def _fill_msg(msg, dic): def document_to_msg_and_meta(document, TYPE): """ + TODO: Remove? Given a document in the database, return metadata and ROS message -- must have been """ meta = document["_meta"] @@ -186,6 +191,7 @@ def document_to_msg_and_meta(document, TYPE): def document_to_msg(document, TYPE): """ + TODO: Remove? Given a document return ROS message """ msg = TYPE() @@ -265,7 +271,7 @@ def sanitize_value(attr, v, type): return v -def store_message(collection, msg, meta, oid=None): +def store_message(collection: pymongo.collection.Collection, msg, meta, oid=None): """ Update ROS message into the DB @@ -277,18 +283,13 @@ def store_message(collection, msg, meta, oid=None): :Returns: | str: ObjectId of the MongoDB document. """ - doc = msg_to_document(msg) + doc = yaml.safe_load(rosidl_runtime_py.message_to_yaml(msg)) + + message_type = message_to_namespaced_type(msg) doc["_meta"] = meta # also store type information - doc["_meta"]["stored_class"] = msg.__module__ + "." + msg.__class__.__name__ - doc["_meta"]["stored_type"] = msg._type - - if ( - msg._type == "soma2_msgs/SOMA2Object" - or msg._type == "soma_msgs/SOMAObject" - or msg._type == "soma_msgs/SOMAROIObject" - ): - add_soma_fields(msg, doc) + doc["_meta"]["stored_class"] = ".".join([msg.__module__, msg.__class__.__name__]) + doc["_meta"]["stored_type"] = message_type if hasattr(msg, "_connection_header"): print(getattr(msg, "_connection_header")) @@ -296,17 +297,7 @@ def store_message(collection, msg, meta, oid=None): if oid != None: doc["_id"] = oid - return collection.insert(doc) - - -# """ -# Stores a ROS message into the DB with msg and meta as separate fields -# """ -# def store_message_separate(collection, msg, meta): -# doc={} -# doc["_meta"]=meta -# doc["msg"]=msg_to_document(msg) -# return collection.insert(doc) + return collection.insert_one(doc) def store_message_no_meta(collection, msg): @@ -320,93 +311,7 @@ def store_message_no_meta(collection, msg): | str: The ObjectId of the MongoDB document created. """ doc = msg_to_document(msg) - return collection.insert(doc) - - -def fill_message(message, document): - """ - Fill a ROS message from a dictionary, assuming the slots of the message are keys in the dictionary. - - :Args: - | message (ROS message): An instance of a ROS message that will be filled in - | document (dict): A dicionary containing all of the message attributes - - Example: - - >>> from geometry_msgs.msg import Pose - >>> d = dcu.msg_to_document(Pose()) - >>> d['position']['x']=27.0 - >>> new_pose = Pose( - >>> fill_message(new_pose, d) - >>> new_pose - position: - x: 27.0 - y: 0.0 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 0.0 - """ - for slot, slot_type in zip( - message.__slots__, - getattr(message, "SLOT_TYPES", [""] * len(message.__slots__)), - ): - - # This check is required since objects returned with projection queries can have absent keys - if slot in document.keys(): - value = document[slot] - # fill internal structures if value is a dictionary itself - if isinstance(value, dict): - fill_message(getattr(message, slot), value) - elif isinstance(value, list) and slot_type.find("/") != -1: - # if its a list and the type is some message (contains a "/") - lst = [] - # Remove [] from message type ([:-2]) - msg_type = type_to_class_string(slot_type[:-2]) - msg_class = load_class(msg_type) - for i in value: - msg = msg_class() - fill_message(msg, i) - lst.append(msg) - setattr(message, slot, lst) - else: - setattr(message, slot, value) - - -def dictionary_to_message(dictionary, cls): - """ - Create a ROS message from the given dictionary, using fill_message. - - :Args: - | dictionary (dict): A dictionary containing all of the atributes of the message - | cls (class): The python class of the ROS message type being reconstructed. - :Returns: - An instance of cls with the attributes filled. - - - Example: - - >>> from geometry_msgs.msg import Pose - >>> d = {'orientation': {'w': 0.0, 'x': 0.0, 'y': 0.0, 'z': 0.0}, - 'position': {'x': 27.0, 'y': 0.0, 'z': 0.0}} - >>> dictionary_to_message(d, Pose) - position: - x: 27.0 - y: 0.0 - z: 0.0 - orientation: - x: 0.0 - y: 0.0 - z: 0.0 - w: 0.0 - """ - message = cls() - - fill_message(message, dictionary) - - return message + return collection.insert_one(doc) def query_message( @@ -476,7 +381,9 @@ def query_message( return [result for result in collection.find(query_doc).limit(limit)] -def update_message(collection, query_doc, msg, meta, upsert): +def update_message( + collection: pymongo.collection.Collection, query_doc, msg, meta, upsert +): """ Update ROS message in the DB, return updated id and true if db altered. @@ -503,13 +410,6 @@ def update_message(collection, query_doc, msg, meta, upsert): # convert msg to db document doc = msg_to_document(msg) - if ( - msg._type == "soma2_msgs/SOMA2Object" - or msg._type == "soma_msgs/SOMAObject" - or msg._type == "soma_msgs/SOMAROIObject" - ): - add_soma_fields(msg, doc) - # update _meta doc["_meta"] = result["_meta"] # merge the two dicts, overwiriting elements in doc["_meta"] with elements in meta @@ -519,7 +419,7 @@ def update_message(collection, query_doc, msg, meta, upsert): doc["_meta"]["stored_class"] = msg.__module__ + "." + msg.__class__.__name__ doc["_meta"]["stored_type"] = msg._type - return collection.update(query_doc, doc), True + return collection.update_one(query_doc, doc), True def query_message_ids(collection, query_doc, find_one): @@ -543,44 +443,33 @@ def query_message_ids(collection, query_doc, find_one): ) -def type_to_class_string(type): +def message_to_namespaced_type(message): """ - Takes a ROS msg type and turns it into a Python module and class name. + Takes a ROS msg and turn it into a namespaced type E.g - - >>> type_to_class_string("geometry_msgs/Pose") - geometry_msgs.msg._Pose.Pose + >>> type(Pose()) + + >>> type_to_class_string(Pose()) + geometry_msgs/Pose :Args: - | type (str): The ROS message type to return class string + | type (ROS message): The ROS message object :Returns: | str: A python class string for the ROS message type supplied """ - parts = type.split("/") - cls_string = "%s.msg._%s.%s" % (parts[0], parts[1], parts[1]) + if "metaclass" in str(type(message)).lower(): + # print( + # f"Received message {type(message)} which is a metaclass. You should pass instantiated objects rather than " + # f"metaclasses, but I'll convert it for you." + # ) + message = message() + message_type = type(message) + module_parts = message_type.__module__.split(".") + cls_string = f"{module_parts[0]}/{module_parts[1]}/{message_type.__name__}" return cls_string -def load_class(full_class_string): - """ - Dynamically load a class from a string - shamelessly ripped from: http://thomassileo.com/blog/2012/12/21/dynamically-load-python-modules-or-classes/ - - :Args: - | full_class_string (str): The python class to dynamically load - :Returns: - | class: the loaded python class. - """ - # todo: cache classes (if this is an overhead) - class_data = full_class_string.split(".") - module_path = ".".join(class_data[:-1]) - class_str = class_data[-1] - module = importlib.import_module(module_path) - # Finally, we retrieve the Class - return getattr(module, class_str) - - def serialise_message(message): """ Create a mongodb_store_msgs/SerialisedMessage instance from a ROS message. @@ -590,11 +479,11 @@ def serialise_message(message): :Returns: | mongodb_store_msgs.msg.SerialisedMessage: A serialised copy of message """ - buf = Buffer() - message.serialize(buf) + msg_bytes = rclpy.serialization.serialize_message(message) serialised_msg = SerialisedMessage() - serialised_msg.msg = buf.getvalue() - serialised_msg.type = message._type + serialised_msg.msg = msg_bytes + serialised_msg.type = message_to_namespaced_type(message) + return serialised_msg @@ -607,12 +496,10 @@ def deserialise_message(serialised_message): :Returns: | ROS message: The message deserialised """ - cls_string = type_to_class_string(serialised_message.type) - cls = load_class(cls_string) - # instantiate an object from the class - message = cls() - # deserialize data into object - message.deserialize(serialised_message.msg) + cls = rosidl_runtime_py.utilities.get_interface(serialised_message.type) + message = rclpy.serialization.deserialize_message( + bytes(serialised_message.msg), cls + ) return message @@ -640,7 +527,8 @@ def string_pair_list_to_dictionary(spl): """ if len(spl.pairs) > 0 and spl.pairs[0].first == MongoQueryMsg.Request.JSON_QUERY: # print "looks like %s", spl.pairs[0].second - return json.loads(spl.pairs[0].second, object_hook=json_util.object_hook) + # json loads will return None if the pair value is 'null'. Make sure it returns a dict. + return json.loads(spl.pairs[0].second, object_hook=json_util.object_hook) or {} # else use the string pairs else: return string_pair_list_to_dictionary_no_json(spl.pairs) @@ -651,31 +539,3 @@ def topic_name_to_collection_name(topic_name): Converts the fully qualified name of a topic into legal mongodb collection name. """ return topic_name.replace("/", "_")[1:] - - -def add_soma_fields(msg, doc): - """ - For soma Object msgs adds the required fields as indexes to the mongodb object. - """ - - if hasattr(msg, "pose"): - doc["loc"] = [doc["pose"]["position"]["x"], doc["pose"]["position"]["y"]] - if hasattr(msg, "logtimestamp"): - doc["timestamp"] = datetime.fromtimestamp(doc["logtimestamp"], timezone.utc) - # doc["timestamp"] = datetime.strptime(doc["logtime"], "%Y-%m-%dT%H:%M:%SZ") - - if hasattr(msg, "geotype"): - if doc["geotype"] == "Point": - for p in doc["geoposearray"]["poses"]: - doc["geoloc"] = { - "type": doc["geotype"], - "coordinates": [p["position"]["x"], p["position"]["y"]], - } - if msg._type == "soma_msgs/SOMAROIObject": - coordinates = [] - doc["geotype"] = "Polygon" - for p in doc["geoposearray"]["poses"]: - coordinates.append([p["position"]["x"], p["position"]["y"]]) - coordinates2 = [] - coordinates2.append(coordinates) - doc["geoloc"] = {"type": doc["geotype"], "coordinates": coordinates2} From 9b4059154296ca270c6421dd96ad1b064a0bc1e6 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Tue, 15 Apr 2025 16:02:09 +0100 Subject: [PATCH 18/25] query, insert, delete, update functional - example works in ros2 util - message conversion uses rosidl_runtime_py rather than custom conversion functions - store the message in message field rather than with the metadata to make it easy to use the runtime py functions - message update uses the $set command message store node - add decorator to make sure crashing callback threads actually output something rather than crashing silently - fix insertion object id return - use delete_one instead of remove - update no longer stores caller id - query correctly instantiates message message store - using the async service call in message store proxy to ensure it works when rclpy spin isn't called - fix update named crash when meta was none --- mongodb_store/mongodb_store/message_store.py | 36 ++--- .../mongodb_store/message_store_node.py | 37 ++++-- .../scripts/example_message_store_client.py | 124 +++++++++++------- mongodb_store/mongodb_store/util.py | 62 ++------- 4 files changed, 135 insertions(+), 124 deletions(-) diff --git a/mongodb_store/mongodb_store/message_store.py b/mongodb_store/mongodb_store/message_store.py index ab58aa6..e2a813b 100644 --- a/mongodb_store/mongodb_store/message_store.py +++ b/mongodb_store/mongodb_store/message_store.py @@ -1,17 +1,19 @@ -import rclpy +import copy +import json import typing + +import rclpy +from bson import json_util +from bson.objectid import ObjectId + +import mongodb_store.util as dc_util +from mongodb_store_msgs.msg import StringPair, StringPairList, Insert from mongodb_store_msgs.srv import ( MongoInsertMsg, MongoDeleteMsg, MongoQueryMsg, MongoUpdateMsg, ) -import mongodb_store.util as dc_util -from mongodb_store_msgs.msg import StringPair, StringPairList, SerialisedMessage, Insert -from bson import json_util -from bson.objectid import ObjectId -import json -import copy class MessageStoreProxy: @@ -179,7 +181,9 @@ def delete(self, message_id: str) -> bool: request.database = self.database request.collection = self.collection request.document_id = message_id - return self.delete_srv.call(request) + return dc_util.check_and_get_service_result_async( + self.parent_node, self.delete_srv, request + )[0].success def query_named( self, @@ -235,6 +239,8 @@ def update_named( meta_query["name"] = name # make sure the name goes into the meta info after update + if meta is None: + meta = {} meta_copy = copy.copy(meta) meta_copy["name"] = name @@ -267,7 +273,7 @@ def update( message_query: typing.Dict = None, meta_query: typing.Dict = None, upsert: bool = False, - ): + ) -> MongoUpdateMsg.Response: """ Updates a message. @@ -318,11 +324,9 @@ def update( request.message = dc_util.serialise_message(message) request.meta = StringPairList(pairs=meta_tuple) - return self.update_srv.call(request) - - """ - Returns [message, meta] where message is the queried message and meta a dictionary of meta information. If single is false returns a list of these lists. - """ + return dc_util.check_and_get_service_result_async( + self.parent_node, self.update_srv, request + )[0] def query( self, @@ -405,7 +409,9 @@ def query( request.projection_query = projection_tuple request.sort_query = sort_tuple - response = self.query_srv.call(request) + response = dc_util.check_and_get_service_result_async( + self.parent_node, self.query_srv, request + )[0] if response.messages is None: messages = [] diff --git a/mongodb_store/mongodb_store/message_store_node.py b/mongodb_store/mongodb_store/message_store_node.py index 569f087..bbccf86 100755 --- a/mongodb_store/mongodb_store/message_store_node.py +++ b/mongodb_store/mongodb_store/message_store_node.py @@ -27,9 +27,25 @@ MongoQuerywithProjectionMsg, ) +import functools + MongoClient = dc_util.import_MongoClient() + +def srv_call_decorator(func): + @functools.wraps(func) + def wrapper(*args, **kwargs): + try: + print("executing wrapped") + return func(*args, **kwargs) + except Exception as e: + import traceback + print(traceback.print_exc()) + + return wrapper + + class MessageStore(rclpy.node.Node): def __init__(self, replicate_on_write=False): super().__init__("message_store") @@ -158,6 +174,7 @@ def insert_ros_msg(self, msg): # actually procedure is the same self.insert_ros_srv(msg, MongoInsertMsg.Response()) + @srv_call_decorator def insert_ros_srv( self, request: MongoInsertMsg.Request, response: MongoInsertMsg.Response ) -> MongoInsertMsg.Response: @@ -211,7 +228,7 @@ def insert_ros_srv( meta["published_at"] = datetime.fromtimestamp(fl, timezone.utc) meta["timestamp"] = stamp.nanoseconds - obj_id = dc_util.store_message(collection, obj, meta) + obj_id = dc_util.store_message(collection, obj, meta).inserted_id return MongoInsertMsg.Response(id=str(obj_id)) except Exception as e: import traceback @@ -221,6 +238,7 @@ def insert_ros_srv( insert_ros_srv.type = MongoInsertMsg + @srv_call_decorator def delete_ros_srv( self, request: MongoDeleteMsg.Request, response: MongoDeleteMsg.Response ) -> MongoDeleteMsg.Response: @@ -238,19 +256,20 @@ def delete_ros_srv( message = docs[0] # Remove the doc - collection.remove({"_id": ObjectId(request.document_id)}) + collection.delete_one({"_id": ObjectId(request.document_id)}) if self.keep_trash: - # But keep it into "trash" + # But keep it in "trash" bk_collection = self._mongo_client[request.database][ request.collection + "_Trash" ] - bk_collection.save(message) + bk_collection.insert_one(message) return MongoDeleteMsg.Response(success=True) delete_ros_srv.type = MongoDeleteMsg + @srv_call_decorator def update_ros_srv( self, request: MongoUpdateMsg.Request, response: MongoUpdateMsg.Response ) -> MongoUpdateMsg.Response: @@ -277,7 +296,8 @@ def update_ros_srv( meta["last_updated_at"] = datetime.fromtimestamp( self.get_clock().now().seconds_nanoseconds()[0], timezone.utc ) - meta["last_updated_by"] = request._connection_header["callerid"] + # can't do this in ros2 + # meta["last_updated_by"] = request._connection_header["callerid"] (obj_id, altered) = dc_util.update_message( collection, obj_query, obj, meta, request.upsert @@ -297,6 +317,7 @@ def to_query_dict(self, message_query, meta_query): obj_query["_meta." + k] = v return obj_query + @srv_call_decorator def query_messages_ros_srv( self, request: MongoQueryMsg.Request, response: MongoQueryMsg.Response ) -> MongoQueryMsg.Response: @@ -353,14 +374,14 @@ def query_messages_ros_srv( metas = () for idx, entry in enumerate(entries): - # load the class object for this type # TODO this should be the same for every item in the list, so could reuse cls = rosidl_runtime_py.utilities.get_interface( - entry["_meta"]["stored_class"] + entry["_meta"]["stored_type"] ) # instantiate the ROS message object from the dictionary retrieved from the db - message = rosidl_runtime_py.set_message_fields(cls(), entry) + message = cls() + rosidl_runtime_py.set_message_fields(message, entry["message"]) # the serialise this object in order to be sent in a generic form serialised_messages = serialised_messages + ( dc_util.serialise_message(message), diff --git a/mongodb_store/mongodb_store/scripts/example_message_store_client.py b/mongodb_store/mongodb_store/scripts/example_message_store_client.py index 1e25335..34b443b 100755 --- a/mongodb_store/mongodb_store/scripts/example_message_store_client.py +++ b/mongodb_store/mongodb_store/scripts/example_message_store_client.py @@ -1,85 +1,109 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -from __future__ import print_function -import rospy -import mongodb_store_msgs.srv as dc_srv -import mongodb_store.util as dc_util -from mongodb_store.message_store import MessageStoreProxy +import sys +import rclpy from geometry_msgs.msg import Pose, Point, Quaternion -import platform -if float(platform.python_version()[0:2]) >= 3.0: - import io -else: - import StringIO - - -if __name__ == '__main__': - rospy.init_node("example_message_store_client") - - - msg_store = MessageStoreProxy() - - p = Pose(Point(0, 1, 2), Quaternion(3, 4, 5, 6)) - +from mongodb_store.message_store import MessageStoreProxy +from mongodb_store.util import message_to_namespaced_type + +if __name__ == "__main__": + rclpy.init() + node = rclpy.node.Node("example_message_store_client") + + msg_store = MessageStoreProxy(node) + print("Created message store proxy") + p = Pose( + position=Point(x=0.0, y=1.0, z=2.0), + orientation=Quaternion(x=3.0, y=4.0, z=5.0, w=6.0), + ) try: - - # insert a pose object with a name, store the id from db - p_id = msg_store.insert_named("my favourite pose", p) + print("Inserting pose") + p_id_fav = msg_store.insert_named("my favourite pose", p) # you don't need a name (note that this p_id is different than one above) - p_id = msg_store.insert(p) + p_id_other = msg_store.insert(p) # p_id = msg_store.insert(['test1', 'test2']) # get it back with a name - print(msg_store.query_named("my favourite pose", Pose._type)) + print("querying") + print( + f"query result: {msg_store.query_named('my favourite pose', message_to_namespaced_type(Pose))}" + ) - p.position.x = 666 + p.position.x = 666.0 # update it with a name + print("Updating named") msg_store.update_named("my favourite pose", p) - p.position.y = 2020 + p.position.y = 2020.0 # update the other inserted one using the id - msg_store.update_id(p_id, p) + print("updating with id") + msg_store.update_id(p_id_other, p) + + stored_p, meta = msg_store.query_id(p_id_other, message_to_namespaced_type(Pose)) - stored_p, meta = msg_store.query_id(p_id, Pose._type) + print(stored_p, meta) assert stored_p.position.x == 666 assert stored_p.position.y == 2020 print("stored object ok") - print("stored object inserted at %s (UTC rostime) by %s" % (meta['inserted_at'], meta['inserted_by'])) - print("stored object last updated at %s (UTC rostime) by %s" % (meta['last_updated_at'], meta['last_updated_by'])) + print(f"stored object inserted at {meta['inserted_at']} (UTC rostime)") + print(f"stored object last updated at {meta['last_updated_at']} (UTC rostime)") # some other things you can do... # get it back with a name - print(msg_store.query_named("my favourite pose", Pose._type)) - + print("Getting back the object using its name") + print( + msg_store.query_named("my favourite pose", message_to_namespaced_type(Pose)) + ) # try to get it back with an incorrect name, so get None instead - print(msg_store.query_named("my favourite position", Pose._type)) + print("Trying to get back the object with an incorrect name") + print( + msg_store.query_named( + "my favourite position", message_to_namespaced_type(Pose) + ) + ) # get all poses - print(msg_store.query(Pose._type)) + print("Getting all poses") + print(msg_store.query(message_to_namespaced_type(Pose))) # get the latest one pose - print(msg_store.query(Pose._type, sort_query=[("$natural", -1)], single=True)) - - # get all non-existant typed objects, so get an empty list back - print(msg_store.query( "not my type")) + print("Getting the latest pose") + print( + msg_store.query( + message_to_namespaced_type(Pose), + sort_query=[("$natural", -1)], + single=True, + ) + ) + + # get all non-existent typed objects, so get an empty list back + print("Getting objects with a non-existent type") + print(msg_store.query("not my type")) # get all poses where the y position is 1 - print(msg_store.query(Pose._type, {"position.y": 1})) + print("All poses where the y position is 1") + print(msg_store.query(message_to_namespaced_type(Pose), {"message.position.y": 1.0})) # get all poses where the y position greater than 0 - print(msg_store.query(Pose._type, {"position.y": {"$gt": 0}})) - - - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - - - + print("All poses where the y position is greater than 0") + print( + msg_store.query( + message_to_namespaced_type(Pose), {"message.position.y": {"$gt": 0}} + ) + ) + + print("deleting my favourite pose...") + msg_store.delete(p_id_fav) + print("deleting the other pose...") + msg_store.delete(p_id_other) + + except Exception: + import traceback + + print(traceback.print_exc()) diff --git a/mongodb_store/mongodb_store/util.py b/mongodb_store/mongodb_store/util.py index b0b6470..d1cdd81 100644 --- a/mongodb_store/mongodb_store/util.py +++ b/mongodb_store/mongodb_store/util.py @@ -166,39 +166,6 @@ def mongo_client_wrapper(*args, **kwargs): return mongo_client_wrapper -def _fill_msg(msg, dic): - """ - TODO: Remove? - Given a ROS msg and a dictionary of the right values, fill in the msg - """ - for i in dic: - if isinstance(dic[i], dict): - _fill_msg(getattr(msg, i), dic[i]) - else: - setattr(msg, i, dic[i]) - - -def document_to_msg_and_meta(document, TYPE): - """ - TODO: Remove? - Given a document in the database, return metadata and ROS message -- must have been - """ - meta = document["_meta"] - msg = TYPE() - _fill_msg(msg, document["msg"]) - return meta, msg - - -def document_to_msg(document, TYPE): - """ - TODO: Remove? - Given a document return ROS message - """ - msg = TYPE() - _fill_msg(msg, document) - return msg - - def msg_to_document(msg): """ Given a ROS message, turn it into a (nested) dictionary suitable for the datacentre. @@ -213,19 +180,7 @@ def msg_to_document(msg): :Returns: | dict : A dictionary representation of the supplied message. """ - - d = {} - - slot_types = [] - if hasattr(msg, "SLOT_TYPES"): - slot_types = msg.SLOT_TYPES - else: - slot_types = [None] * len(msg.__slots__) - - for attr, type in zip(msg._fields_and_string_types.keys(), slot_types): - d[attr] = sanitize_value(attr, getattr(msg, attr), type) - - return d + return yaml.safe_load(rosidl_runtime_py.message_to_yaml(msg)) def sanitize_value(attr, v, type): @@ -283,7 +238,9 @@ def store_message(collection: pymongo.collection.Collection, msg, meta, oid=None :Returns: | str: ObjectId of the MongoDB document. """ - doc = yaml.safe_load(rosidl_runtime_py.message_to_yaml(msg)) + # The message should be stored separate from the meta fields so it can be more easily restored. Otherwise rosidl + # dict to message has problems later because the message doesn't have the meta fields + doc = {"message": msg_to_document(msg)} message_type = message_to_namespaced_type(msg) doc["_meta"] = meta @@ -408,7 +365,7 @@ def update_message( return "", False # convert msg to db document - doc = msg_to_document(msg) + doc = {"message": msg_to_document(msg)} # update _meta doc["_meta"] = result["_meta"] @@ -416,10 +373,13 @@ def update_message( doc["_meta"] = dict(list(doc["_meta"].items()) + list(meta.items())) # ensure necessary parts are there too - doc["_meta"]["stored_class"] = msg.__module__ + "." + msg.__class__.__name__ - doc["_meta"]["stored_type"] = msg._type + doc["_meta"]["stored_class"] = ".".join([msg.__module__, msg.__class__.__name__]) + doc["_meta"]["stored_type"] = message_to_namespaced_type(msg) + + # Have to use the $set command to actually update the matching entry + set_cmd = {"$set": doc} - return collection.update_one(query_doc, doc), True + return collection.update_one(query_doc, set_cmd), True def query_message_ids(collection, query_doc, find_one): From ea49ea7f7c0788dd7f9d4d12a0b0213db796533a Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Tue, 15 Apr 2025 16:24:20 +0100 Subject: [PATCH 19/25] move mongodb server --- mongodb_store/mongodb_store/{scripts => }/mongodb_server.py | 0 mongodb_store/setup.py | 2 +- 2 files changed, 1 insertion(+), 1 deletion(-) rename mongodb_store/mongodb_store/{scripts => }/mongodb_server.py (100%) diff --git a/mongodb_store/mongodb_store/scripts/mongodb_server.py b/mongodb_store/mongodb_store/mongodb_server.py similarity index 100% rename from mongodb_store/mongodb_store/scripts/mongodb_server.py rename to mongodb_store/mongodb_store/mongodb_server.py diff --git a/mongodb_store/setup.py b/mongodb_store/setup.py index 20c6e56..a4c24fe 100644 --- a/mongodb_store/setup.py +++ b/mongodb_store/setup.py @@ -54,7 +54,7 @@ def glob_files( "message_store_node = mongodb_store.message_store_node:main", "mongo_bridge = mongodb_store.scripts.mongo_bridge:main", "mongodb_play = mongodb_store.scripts.mongodb_play:main", - "mongodb_server = mongodb_store.scripts.mongodb_server:main", + "mongodb_server = mongodb_store.mongodb_server:main", "replicator_client= mongodb_store.scripts.replicator_client:main", "replicator_node = mongodb_store.scripts.replicator_node:main", ], From 9a44ef3ca8094c961cf3ddc25d53713b5ad12ed0 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Tue, 15 Apr 2025 16:26:19 +0100 Subject: [PATCH 20/25] multi event log example works --- mongodb_store/mongodb_store/message_store.py | 2 +- .../scripts/example_multi_event_log.py | 107 ++++++++++-------- 2 files changed, 59 insertions(+), 50 deletions(-) diff --git a/mongodb_store/mongodb_store/message_store.py b/mongodb_store/mongodb_store/message_store.py index e2a813b..2a41c1f 100644 --- a/mongodb_store/mongodb_store/message_store.py +++ b/mongodb_store/mongodb_store/message_store.py @@ -337,7 +337,7 @@ def query( sort_query: typing.List[typing.Tuple] = None, projection_query: typing.Dict = None, limit: int = 0, - ): + ) -> typing.Tuple[typing.Any, typing.Dict]: """ Finds and returns message(s) matching the message and meta data queries. diff --git a/mongodb_store/mongodb_store/scripts/example_multi_event_log.py b/mongodb_store/mongodb_store/scripts/example_multi_event_log.py index ce84623..51fc2f2 100755 --- a/mongodb_store/mongodb_store/scripts/example_multi_event_log.py +++ b/mongodb_store/mongodb_store/scripts/example_multi_event_log.py @@ -1,69 +1,78 @@ -#!/usr/bin/env python -from __future__ import print_function -import rospy -from mongodb_store_msgs.msg import StringPairList, StringPair -import mongodb_store_msgs.srv as dc_srv -import mongodb_store.util as dc_util -from mongodb_store.message_store import MessageStoreProxy +from datetime import * + +import rclpy from geometry_msgs.msg import Pose, Point, Quaternion from std_msgs.msg import Bool -from datetime import * -import platform -if float(platform.python_version()[0:2]) >= 3.0: - import io -else: - import StringIO -if __name__ == '__main__': - rospy.init_node("example_multi_event_log") +from mongodb_store.message_store import MessageStoreProxy +from mongodb_store_msgs.msg import StringPairList, StringPair +from mongodb_store.util import message_to_namespaced_type + +if __name__ == "__main__": + rclpy.init() + node = rclpy.node.Node("example_multi_event_log") try: # let's say we have a couple of things that we need to store together # these could be some sensor data, results of processing etc. - pose = Pose(Point(0, 1, 2), Quaternion(3, 4, 5, 6)) - point = Point(7, 8, 9) - quaternion = Quaternion(10, 11, 12, 13) + pose = Pose( + position=Point(x=0.0, y=1.0, z=2.0), + orientation=Quaternion(x=3.0, y=4.0, z=5.0, w=6.0), + ) + point = Point(x=7.0, y=8.0, z=9.0) + quaternion = Quaternion(x=10.0, y=11.0, z=12.0, w=13.0) # note that everything that is pass to the message_store must be a ros message type - #therefore use std_msg types for standard data types like float, int, bool, string etc - result = Bool(True) - + # therefore use std_msg types for standard data types like float, int, bool, string etc + result = Bool(data=True) # we will store our results in a separate collection - msg_store = MessageStoreProxy(collection='pose_results') - # save the ids from each addition - stored = [] - stored.append([pose._type, msg_store.insert(pose)]) - stored.append([point._type, msg_store.insert(point)]) - stored.append([quaternion._type, msg_store.insert(quaternion)]) - stored.append([result._type, msg_store.insert(result)]) + msg_store = MessageStoreProxy(node, collection="pose_results") - # now store ids togther in store, addition types for safety + messages_to_store = [pose, point, quaternion, result] spl = StringPairList() - for pair in stored: - spl.pairs.append(StringPair(pair[0], pair[1])) + for message in messages_to_store: + # Each pair in the string pair list will be the type of message stored, and the id of the relevant + # message in the collection + spl.pairs.append( + StringPair( + first=message_to_namespaced_type(message), + second=msg_store.insert(message), + ) + ) # and add some meta information - meta = {} - meta['description'] = "this wasn't great" - meta['result_time'] = datetime.utcfromtimestamp(rospy.get_rostime().to_sec()) - msg_store.insert(spl, meta = meta) + meta = {"description": "this wasn't great"} + sec_ns = node.get_clock().now().seconds_nanoseconds() + fl = float(f"{sec_ns[0]}.{sec_ns[1]}") + meta["result_time"] = datetime.fromtimestamp(fl, timezone.utc) + msg_store.insert(spl, meta=meta) # now let's get all our logged data back - results = msg_store.query(StringPairList._type) + results = msg_store.query(message_to_namespaced_type(StringPairList)) for message, meta in results: - if 'description' in meta: - print('description: %s' % meta['description']) - print('result time (UTC from rostime): %s' % meta['result_time']) - print('inserted at (UTC from rostime): %s' % meta['inserted_at']) - pose = msg_store.query_id(message.pairs[0].second, Pose._type) - point = msg_store.query_id(message.pairs[1].second, Point._type) - quaternion = msg_store.query_id(message.pairs[2].second, Quaternion._type) - result = msg_store.query_id(message.pairs[3].second, Bool._type) - - - except rospy.ServiceException as e: - print("Service call failed: %s"%e) - + if "description" in meta: + print(f"description: {meta['description']}") + print(f"result time (UTC from rostime): {meta['result_time']}") + print(f"inserted at (UTC from rostime): {meta['inserted_at']}") + pose = msg_store.query_id( + message.pairs[0].second, message_to_namespaced_type(Pose) + )[0] + point = msg_store.query_id( + message.pairs[1].second, message_to_namespaced_type(Point) + )[0] + quaternion = msg_store.query_id( + message.pairs[2].second, message_to_namespaced_type(Quaternion) + )[0] + result = msg_store.query_id( + message.pairs[3].second, message_to_namespaced_type(Bool) + )[0] + print(pose) + print(point) + print(quaternion) + print(result) + except Exception: + import traceback + print(traceback.print_exc()) From cd1eda7f83038637466959c1f5098d6ec837162f Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Tue, 15 Apr 2025 17:03:41 +0100 Subject: [PATCH 21/25] launch files work, had to change replicator to string --- .../launch/mongodb_store_inc_launch.xml | 135 +++++++++--------- mongodb_store/launch/mongodb_store_launch.xml | 87 ++++++----- mongodb_store/mongodb_store/mongodb_server.py | 6 +- 3 files changed, 108 insertions(+), 120 deletions(-) diff --git a/mongodb_store/launch/mongodb_store_inc_launch.xml b/mongodb_store/launch/mongodb_store_inc_launch.xml index 736bca6..fec033d 100644 --- a/mongodb_store/launch/mongodb_store_inc_launch.xml +++ b/mongodb_store/launch/mongodb_store_inc_launch.xml @@ -1,74 +1,69 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mongodb_store/launch/mongodb_store_launch.xml b/mongodb_store/launch/mongodb_store_launch.xml index 0cfb67b..cb3231e 100644 --- a/mongodb_store/launch/mongodb_store_launch.xml +++ b/mongodb_store/launch/mongodb_store_launch.xml @@ -1,50 +1,43 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/mongodb_store/mongodb_store/mongodb_server.py b/mongodb_store/mongodb_store/mongodb_server.py index aa5c5c9..44446e6 100755 --- a/mongodb_store/mongodb_store/mongodb_server.py +++ b/mongodb_store/mongodb_store/mongodb_server.py @@ -43,7 +43,7 @@ def __init__(self): "test_mode", False, descriptor=ParameterDescriptor(description="") ).value self.repl_set = self.declare_parameter( - "repl_set", None, descriptor=ParameterDescriptor(description="") + "repl_set", "", descriptor=ParameterDescriptor(description="") ).value self.bind_to_host = self.declare_parameter( "bind_to_host", False, descriptor=ParameterDescriptor(description="") @@ -155,7 +155,7 @@ def block_mongo_kill(): cmd.append("--bind_ip") cmd.append("0.0.0.0") - if self.repl_set is not None: + if self.repl_set: cmd.append("--replSet") cmd.append(self.repl_set) @@ -180,7 +180,7 @@ def block_mongo_kill(): if not self._ready and stdout.find("mongod startup complete") != -1: self._ready = True - if self.repl_set is not None: + if self.repl_set: try: self.initialize_repl_set() except Exception as e: From c5bcab7629580f30af60fabb3ecc9d1ffdb0e7e1 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Wed, 16 Apr 2025 13:45:34 +0100 Subject: [PATCH 22/25] fix reading stamps from header and tf --- mongodb_store/mongodb_store/message_store_node.py | 11 ++++++----- mongodb_store/mongodb_store/mongodb_server.py | 9 ++++----- mongodb_store/mongodb_store/util.py | 14 ++++---------- 3 files changed, 14 insertions(+), 20 deletions(-) diff --git a/mongodb_store/mongodb_store/message_store_node.py b/mongodb_store/mongodb_store/message_store_node.py index bbccf86..d0b4c00 100755 --- a/mongodb_store/mongodb_store/message_store_node.py +++ b/mongodb_store/mongodb_store/message_store_node.py @@ -9,6 +9,7 @@ import pymongo import rclpy +import rclpy.time from bson import json_util from bson.objectid import ObjectId from builtin_interfaces.msg import Time @@ -32,7 +33,6 @@ MongoClient = dc_util.import_MongoClient() - def srv_call_decorator(func): @functools.wraps(func) def wrapper(*args, **kwargs): @@ -41,6 +41,7 @@ def wrapper(*args, **kwargs): return func(*args, **kwargs) except Exception as e: import traceback + print(traceback.print_exc()) return wrapper @@ -215,13 +216,13 @@ def insert_ros_srv( and hasattr(obj.header, "stamp") and isinstance(obj.header.stamp, Time) ): - stamp = obj.header.stamp + stamp = rclpy.time.Time.from_msg(obj.header.stamp) elif isinstance(obj, TFMessage): if obj.transforms: transforms = sorted( obj.transforms, key=lambda m: m.header.stamp, reverse=True ) - stamp = transforms[0].header.stamp + stamp = rclpy.time.Time.from_msg(transforms[0].header.stamp) sec_ns = stamp.seconds_nanoseconds() fl = float(f"{sec_ns[0]}.{sec_ns[1]}") @@ -233,7 +234,7 @@ def insert_ros_srv( except Exception as e: import traceback - print(traceback.format_exc()) + self.get_logger().error(traceback.format_exc()) return MongoInsertMsg.Response(id="") insert_ros_srv.type = MongoInsertMsg @@ -377,7 +378,7 @@ def query_messages_ros_srv( # load the class object for this type # TODO this should be the same for every item in the list, so could reuse cls = rosidl_runtime_py.utilities.get_interface( - entry["_meta"]["stored_type"] + entry["_meta"]["stored_type"] ) # instantiate the ROS message object from the dictionary retrieved from the db message = cls() diff --git a/mongodb_store/mongodb_store/mongodb_server.py b/mongodb_store/mongodb_store/mongodb_server.py index 44446e6..02df6a1 100755 --- a/mongodb_store/mongodb_store/mongodb_server.py +++ b/mongodb_store/mongodb_store/mongodb_server.py @@ -173,10 +173,10 @@ def block_mongo_kill(): else: raise if stdout is not None: - if stdout.find("ERROR") != -1: - self.get_logger().error(stdout.strip()) - else: - self.get_logger().info(stdout.strip()) + # if stdout.find("ERROR") != -1: + # self.get_logger().error(stdout.strip()) + # else: + # self.get_logger().info(stdout.strip()) if not self._ready and stdout.find("mongod startup complete") != -1: self._ready = True @@ -209,7 +209,6 @@ def _wait_ready_srv_cb( self, request: Empty.Request, resp: Empty.Response ) -> Empty.Response: while not self._ready: - self.get_logger().info("waiting") self.get_clock().sleep_for(Duration(seconds=0.1)) return Empty.Response() diff --git a/mongodb_store/mongodb_store/util.py b/mongodb_store/mongodb_store/util.py index d1cdd81..4c47d58 100644 --- a/mongodb_store/mongodb_store/util.py +++ b/mongodb_store/mongodb_store/util.py @@ -1,22 +1,16 @@ -import importlib import json - -import pymongo.collection -import yaml import typing -from datetime import datetime -from datetime import timezone -from io import BytesIO as Buffer +import pymongo.collection import rclpy -import rclpy.node import rclpy.client -import rclpy.type_support +import rclpy.node import rclpy.serialization +import rclpy.type_support import rosidl_runtime_py.utilities +import yaml from bson import json_util, Binary from pymongo.errors import ConnectionFailure -from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.executors import MultiThreadedExecutor from std_srvs.srv import Empty From 90073853db8b08dd8d9dc0e2c9bd258e75d0c7b1 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Wed, 16 Apr 2025 15:56:58 +0100 Subject: [PATCH 23/25] ignore log and cxx_ros packages for now --- libmongocxx_ros/COLCON_IGNORE | 0 mongodb_log/COLCON_IGNORE | 0 mongodb_store/mongodb_store/util.py | 2 +- 3 files changed, 1 insertion(+), 1 deletion(-) create mode 100644 libmongocxx_ros/COLCON_IGNORE create mode 100644 mongodb_log/COLCON_IGNORE diff --git a/libmongocxx_ros/COLCON_IGNORE b/libmongocxx_ros/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/mongodb_log/COLCON_IGNORE b/mongodb_log/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/mongodb_store/mongodb_store/util.py b/mongodb_store/mongodb_store/util.py index 4c47d58..19154c5 100644 --- a/mongodb_store/mongodb_store/util.py +++ b/mongodb_store/mongodb_store/util.py @@ -64,7 +64,7 @@ def wait_for_mongo(parent_node: rclpy.node.Node, timeout=60, ns="/datacentre"): wait_client = parent_node.create_client(Empty, service) result, message = check_and_get_service_result_async( - parent_node, wait_client, Empty.Request() + parent_node, wait_client, Empty.Request(), existence_timeout=timeout ) if result is None: parent_node.get_logger().error( From c36b91ea8a62ee7919984fa77d951cb87a7b06c5 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Thu, 24 Apr 2025 13:07:07 +0100 Subject: [PATCH 24/25] fix service waiting in proxy --- mongodb_store/mongodb_store/message_store.py | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) diff --git a/mongodb_store/mongodb_store/message_store.py b/mongodb_store/mongodb_store/message_store.py index 2a41c1f..fbb3ddf 100644 --- a/mongodb_store/mongodb_store/message_store.py +++ b/mongodb_store/mongodb_store/message_store.py @@ -69,18 +69,13 @@ def __init__( self.pub_insert = self.parent_node.create_publisher(Insert, insert_topic, 10) while rclpy.ok(): - try: - self.insert_srv.wait_for_service(5) - self.update_srv.wait_for_service(5) - self.query_srv.wait_for_service(5) - self.delete_srv.wait_for_service(5) - break - except Exception as e: - found_services_first_try = False - self.parent_node.get_logger().error( - "Could not get message store services. Maybe the message " - "store has not been started? Retrying..." - ) + for service in [self.insert_srv, self.update_srv, self.query_srv, self.delete_srv]: + if not service.wait_for_service(5): + found_services_first_try = False + self.parent_node.get_logger().error( + "Could not get message store services. Maybe the message " + "store has not been started? Retrying..." + ) if not found_services_first_try: self.parent_node.get_logger().info("Message store services found.") From f6e0327cf587b8f752fda261c0fbe1a3cc47e624 Mon Sep 17 00:00:00 2001 From: Michal Staniaszek Date: Tue, 29 Apr 2025 10:10:51 +0100 Subject: [PATCH 25/25] fix proxy wait for service infinite loop --- mongodb_store/mongodb_store/message_store.py | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/mongodb_store/mongodb_store/message_store.py b/mongodb_store/mongodb_store/message_store.py index fbb3ddf..ac3c6e4 100644 --- a/mongodb_store/mongodb_store/message_store.py +++ b/mongodb_store/mongodb_store/message_store.py @@ -68,14 +68,23 @@ def __init__( insert_topic = service_prefix + "/insert" self.pub_insert = self.parent_node.create_publisher(Insert, insert_topic, 10) - while rclpy.ok(): - for service in [self.insert_srv, self.update_srv, self.query_srv, self.delete_srv]: + all_ok = False + while rclpy.ok() and not all_ok: + all_ok = True + for service in [ + self.insert_srv, + self.update_srv, + self.query_srv, + self.delete_srv, + ]: if not service.wait_for_service(5): found_services_first_try = False self.parent_node.get_logger().error( - "Could not get message store services. Maybe the message " + f"Could not get message store service {service.srv_name}. Maybe the message " "store has not been started? Retrying..." ) + all_ok = False + if not found_services_first_try: self.parent_node.get_logger().info("Message store services found.")