From e9ebc2211340bcd29f328317e813e434583c94bf Mon Sep 17 00:00:00 2001 From: Sascha Jongebloed Date: Thu, 24 Apr 2025 14:49:14 +0200 Subject: [PATCH 1/4] started writing knowrob ros lib --- CMakeLists.txt | 47 +++++++++++- msg/EventToken.msg | 8 --- package.xml | 3 + scripts/test_knowrob_ros_lib.py | 51 +++++++++++++ setup.py | 10 +++ src/action_streamer.py | 67 ------------------ src/knowrob_ros_lib.py | 122 ++++++++++++++++++++++++++++++++ test/test_knowrob_ros_lib.test | 10 +++ 8 files changed, 241 insertions(+), 77 deletions(-) delete mode 100755 msg/EventToken.msg create mode 100644 scripts/test_knowrob_ros_lib.py create mode 100644 setup.py delete mode 100755 src/action_streamer.py create mode 100644 src/knowrob_ros_lib.py create mode 100644 test/test_knowrob_ros_lib.test diff --git a/CMakeLists.txt b/CMakeLists.txt index 565d552..2fdcc46 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -19,7 +19,16 @@ find_package(catkin REQUIRED COMPONENTS knowrob ) find_package(spdlog REQUIRED) +find_package(rostest REQUIRED) +## ============================== +## Python setup +## ============================== +catkin_python_setup() + +## ============================= +## ROS message generation +## ============================= add_message_files( DIRECTORY msg FILES @@ -29,7 +38,6 @@ add_message_files( TellMessage.msg GraphQueryMessage.msg GraphAnswerMessage.msg - EventToken.msg ) add_action_files( DIRECTORY action @@ -50,7 +58,10 @@ generate_messages(DEPENDENCIES actionlib_msgs ) -catkin_package(CATKIN_DEPENDS roscpp roslib knowrob message_runtime) +## ============================= +## Catkin package +## ============================= +catkin_package(CATKIN_DEPENDS roscpp roslib rospy knowrob message_runtime) include_directories(include ${catkin_INCLUDE_DIRS}) @@ -65,4 +76,36 @@ add_dependencies(knowrob-ros target_link_libraries(knowrob-ros ${catkin_LIBRARIES} spdlog::spdlog) +## ============================= +## Python library installation +## ============================= + +# Install Python library (e.g., knowrob_ros_lib.py) +# With this: +install(FILES + src/knowrob_ros_lib.py + DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} +) + +# Install Python scripts (e.g., test_knowrob_ros_lib.py) +catkin_install_python(PROGRAMS + scripts/test_knowrob_ros_lib.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## ============================= +## Install binaries +## ============================= + install(TARGETS knowrob-ros RUNTIME DESTINATION bin) + +## ============================= +## Install test files +## ============================= + +if (CATKIN_ENABLE_TESTING) + catkin_add_nosetests( + test/test_knowrob_ros_lib.test + DEPENDENCIES ${catkin_EXPORTED_TARGETS} + ) +endif() \ No newline at end of file diff --git a/msg/EventToken.msg b/msg/EventToken.msg deleted file mode 100755 index 9506507..0000000 --- a/msg/EventToken.msg +++ /dev/null @@ -1,8 +0,0 @@ - -uint8 EVENT_BEGIN=0 -uint8 EVENT_END=1 - -float64 timestamp -int32 polarization -string event_type -string[] participants diff --git a/package.xml b/package.xml index 77609f3..f8ccc64 100755 --- a/package.xml +++ b/package.xml @@ -28,11 +28,14 @@ visualization_msgs message_runtime actionlib + rostest + python3 roslib roscpp knowrob spdlog + rospy libmongoc-dev fmt diff --git a/scripts/test_knowrob_ros_lib.py b/scripts/test_knowrob_ros_lib.py new file mode 100644 index 0000000..c63714e --- /dev/null +++ b/scripts/test_knowrob_ros_lib.py @@ -0,0 +1,51 @@ +import knowrob_ros_lib +import unittest +from knowrob_ros.action import AskOneAction, AskOneGoal # Replace with actual import paths + + + +class TestKnowrobRosLib(unittest.TestCase): + # def test_ask_all(self): + # # Test the ask_all function + # result = knowrob_ros.ask_all("lpn:jelous(vincent, X)") + # self.assertEqual(result.bindings, [{ + # 'X': 'hans' + # }]) + + def test_ask_one(self): + # Test the ask_one function + ask_one_result = knowrob_ros.ask_one("lpn:jealous(lpn:vincent, X)") + self.assertTrue(ask_one_result.status == AskOneGoal.TRUE) + result = knowrob_ros_lib.graph_answer_to_dicts(ask_one_result.answer) + self.assertEqual(result.bindings, [{ + 'X': 'lpn:marsellus' + }]) + + # def test_tell(self): + # # Create the triples to be added + # builder = knowrob_ros_lib.TripleQueryBuilder() + # builder.add("alice", "knows", "bob") + # builder.add("bob", "likes", "pizza") + # query_str = builder.build_query_string() + + # # Test the tell function + # result = knowrob_ros.tell(query_str) + # self.assertTrue(result.success) + # result = knowrob_ros.ask_all("lpn:jelous(alice, X)") + # self.assertEqual(result.bindings, [{ + # 'X': 'pizza' + # }]) + + # Init the test class + def setUp(self): + # Initialize the knowrob_ros_lib + self.knowrob_ros = knowrob_ros_lib.KnowRobRosLib() + # Initialize the ROS node + self.knowrob_ros.init_node("test_knowrob_ros_lib") + + def tearDown(self): + # Shutdown the ROS node + self.knowrob_ros.shutdown_node() + +if __name__ == '__main__': + unittest.main() \ No newline at end of file diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..0bfa540 --- /dev/null +++ b/setup.py @@ -0,0 +1,10 @@ +from setuptools import setup + +setup( + name='knowrob_ros', + version='2.0.0', + packages=[], # no packages folders + py_modules=['knowrob_ros_lib'], # installs src/test_lib.py as module test_lib + install_requires=['rospy'], # whatever ROS Python deps you have + # you can also declare entry_points here if you want console_scripts +) diff --git a/src/action_streamer.py b/src/action_streamer.py deleted file mode 100755 index 9f6fe98..0000000 --- a/src/action_streamer.py +++ /dev/null @@ -1,67 +0,0 @@ -#!/usr/bin/env python - -# A simple script to tests sending Event tokens -# for the activity parser and its GUI. - -import sys -import rospy -import time -#import rosprolog_client as rosprolog - -from knowrob.msg._EventToken import EventToken - -class ActionStreamer(object): - def __init__(self): - self.token_publisher = rospy.Publisher('/parser/token', EventToken, queue_size=1) - # HACK: need to wait until publisher is running - rospy.sleep(0.5) - rospy.loginfo('token streamer is running') - - def support(self,obj,location): - self.begin_('Supporting',[location,obj]) - - def pick(self,obj,using,location): - self.begin_('GraspMotion',[using]) - time.sleep(0.2) - self.begin_('Touching',[using,obj]) - time.sleep(0.1) - self.end_('GraspMotion',[using]) - time.sleep(0.4) - self.end_('Supporting',[location,obj]) - - def place(self,obj,using,location): - self.begin_('Supporting',[location,obj]) - time.sleep(0.2) - self.begin_('ReleaseMotion',[using]) - time.sleep(0.1) - self.end_('Touching',[using,obj]) - time.sleep(0.4) - self.end_('ReleaseMotion',[using]) - - def get_iri_(self,x): - return 'http://knowrob.org/kb/parser-test.owl#'+x - - def make_token_(self,tok_polarization,tok_type,tok_objs): - return EventToken( - timestamp=time.time() * 1000.0, - polarization=tok_polarization, - event_type=self.get_iri_(tok_type), - participants=list(map(self.get_iri_,tok_objs)) - ) - - def begin_(self,tok_type,tok_objs): - self.send_token_(self.make_token_(EventToken.EVENT_BEGIN,tok_type,tok_objs)) - - def end_(self,tok_type,tok_objs): - self.send_token_(self.make_token_(EventToken.EVENT_END,tok_type,tok_objs)) - - def send_token_(self,tok): - self.token_publisher.publish(tok) - - -if __name__ == '__main__': - rospy.init_node('parser_stream_test') - streamer = ActionStreamer() - [method_name,rest] = sys.argv[1].split('(') - method_args = dict(map(lambda x: x.split("="),rest[:-1].split(','))) - getattr(streamer, method_name)(**method_args) diff --git a/src/knowrob_ros_lib.py b/src/knowrob_ros_lib.py new file mode 100644 index 0000000..316738f --- /dev/null +++ b/src/knowrob_ros_lib.py @@ -0,0 +1,122 @@ +import rospy +from std_msgs.msg import Bool +from knowrob_ros.msg import GraphQueryMessage, GraphAnswerMessage, KeyValuePair, ModalFrame # Replace with actual import paths +from knowrob_ros.action import AskOneAction, AskOneGoal # Replace with actual import paths +import actionlib + + +class KnowRobRosLib: + def __init__(self): + self._ask_one_client = None + + def init_node(self, name): + rospy.init_node(name, anonymous=True) + self._ask_one_client = actionlib.SimpleActionClient("knowrob/ask_one", AskOneAction) + self._ask_one_client.wait_for_server() + + def shutdown_node(self): + rospy.signal_shutdown("KnowRob node shutdown") + if self._ask_one_client: + self._ask_one_client.cancel_all_goals() + + def ask_one(self, query): + goal = AskOneGoal() + goal.query.query_string = query + self._ask_one_client.send_goal(goal) + self._ask_one_client.wait_for_result() + result = self._ask_one_client.get_result() + return result + + # def tell(self, triples_str): + # request = TellRequest() + # request.query.query_string = triples_str + # response = self._tell_service(request) + # return TellResultAdapter(response) + + # def ask_all(self, query): + # # This is a stub assuming synchronous call, you'd use ROS service or action here too + # # Replace with actual implementation for asking all + # return GraphResultAdapter(GraphAnswerMessage(bindings=[KeyValuePair(key="X", value="hans")])) + + def graph_answer_to_dicts(answer_msg): + """ + Convert a GraphAnswerMessage to a list of dictionaries. + Each dictionary represents one solution, where keys are variable names, + and values are decoded based on the type field. + """ + + results = [] + + for binding_group in answer_msg.bindings: + result = {} + for pair in binding_group.bindings: + if pair.type == KeyValuePair.TYPE_STRING: + result[pair.key] = pair.value_string + elif pair.type == KeyValuePair.TYPE_FLOAT: + result[pair.key] = pair.value_float + elif pair.type == KeyValuePair.TYPE_INT: + result[pair.key] = pair.value_int + elif pair.type == KeyValuePair.TYPE_LONG: + result[pair.key] = pair.value_long + elif pair.type == KeyValuePair.TYPE_VARIABLE: + result[pair.key] = pair.value_variable + elif pair.type == KeyValuePair.TYPE_PREDICATE: + result[pair.key] = pair.value_predicate + elif pair.type == KeyValuePair.TYPE_LIST: + # Lists are stored as a raw string and require custom parsing + result[pair.key] = pair.value_list + else: + result[pair.key] = None # Unknown type + results.append(result) + + return results + +# class GraphResultAdapter: +# def __init__(self, msg): +# self.bindings = ( +# {kv.key: kv.value for kv in msg.bindings} +# if isinstance(msg.bindings, list) +# else msg.bindings +# ) + + +# class TellResultAdapter: +# def __init__(self, response): +# self.success = response.success + + +class TripleQueryBuilder: + def __init__(self): + self.triples = [] + + def add(self, subject, predicate, obj): + """Add a triple to the list.""" + self.triples.append((subject, predicate, obj)) + + def build_query_string(self): + """Generate a Prolog-style query string.""" + return ', '.join(f'{pred}({subj},{obj})' for subj, pred, obj in self.triples) + + +# Module-level functions +_knowrob_instance = KnowRobRosLib() + + +def init_node(name): + _knowrob_instance.init_node(name) + + +def shutdown_node(): + _knowrob_instance.shutdown_node() + + +def ask_one(query): + return _knowrob_instance.ask_one(query) + + +# def ask_all(query): +# return _knowrob_instance.ask_all(query) + + +# def tell(triples_str): +# return _knowrob_instance.tell(triples_str) diff --git a/test/test_knowrob_ros_lib.test b/test/test_knowrob_ros_lib.test new file mode 100644 index 0000000..697da58 --- /dev/null +++ b/test/test_knowrob_ros_lib.test @@ -0,0 +1,10 @@ + + + + + + + + + + From b133561223182d1760ce57287165baafd7539f18 Mon Sep 17 00:00:00 2001 From: Sascha Jongebloed Date: Thu, 24 Apr 2025 15:47:00 +0200 Subject: [PATCH 2/4] fix to import --- CMakeLists.txt | 17 +++-------------- scripts/test_knowrob_ros_lib.py | 4 ++-- setup.py | 10 ---------- src/{ => knowrob_ros_lib}/__init__.py | 0 src/{ => knowrob_ros_lib}/knowrob_ros_lib.py | 10 +++++++--- 5 files changed, 12 insertions(+), 29 deletions(-) delete mode 100644 setup.py rename src/{ => knowrob_ros_lib}/__init__.py (100%) rename src/{ => knowrob_ros_lib}/knowrob_ros_lib.py (93%) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2fdcc46..3ebd0aa 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -21,11 +21,6 @@ find_package(catkin REQUIRED COMPONENTS find_package(spdlog REQUIRED) find_package(rostest REQUIRED) -## ============================== -## Python setup -## ============================== -catkin_python_setup() - ## ============================= ## ROS message generation ## ============================= @@ -82,15 +77,9 @@ target_link_libraries(knowrob-ros ${catkin_LIBRARIES} spdlog::spdlog) # Install Python library (e.g., knowrob_ros_lib.py) # With this: -install(FILES - src/knowrob_ros_lib.py - DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} -) - -# Install Python scripts (e.g., test_knowrob_ros_lib.py) -catkin_install_python(PROGRAMS - scripts/test_knowrob_ros_lib.py - DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +file (COPY + ${CMAKE_CURRENT_SOURCE_DIR}/src/knowrob_ros_lib/knowrob_ros_lib.py + DESTINATION ${CATKIN_DEVEL_PREFIX}/lib/python3/dist-packages/knowrob_ros/ ) ## ============================= diff --git a/scripts/test_knowrob_ros_lib.py b/scripts/test_knowrob_ros_lib.py index c63714e..d4bfdf4 100644 --- a/scripts/test_knowrob_ros_lib.py +++ b/scripts/test_knowrob_ros_lib.py @@ -1,6 +1,6 @@ -import knowrob_ros_lib +import knowrob_ros.knowrob_ros_lib import unittest -from knowrob_ros.action import AskOneAction, AskOneGoal # Replace with actual import paths +from knowrob_ros import AskOneAction, AskOneGoal # Replace with actual import paths diff --git a/setup.py b/setup.py deleted file mode 100644 index 0bfa540..0000000 --- a/setup.py +++ /dev/null @@ -1,10 +0,0 @@ -from setuptools import setup - -setup( - name='knowrob_ros', - version='2.0.0', - packages=[], # no packages folders - py_modules=['knowrob_ros_lib'], # installs src/test_lib.py as module test_lib - install_requires=['rospy'], # whatever ROS Python deps you have - # you can also declare entry_points here if you want console_scripts -) diff --git a/src/__init__.py b/src/knowrob_ros_lib/__init__.py similarity index 100% rename from src/__init__.py rename to src/knowrob_ros_lib/__init__.py diff --git a/src/knowrob_ros_lib.py b/src/knowrob_ros_lib/knowrob_ros_lib.py similarity index 93% rename from src/knowrob_ros_lib.py rename to src/knowrob_ros_lib/knowrob_ros_lib.py index 316738f..19f6943 100644 --- a/src/knowrob_ros_lib.py +++ b/src/knowrob_ros_lib/knowrob_ros_lib.py @@ -1,8 +1,12 @@ import rospy -from std_msgs.msg import Bool -from knowrob_ros.msg import GraphQueryMessage, GraphAnswerMessage, KeyValuePair, ModalFrame # Replace with actual import paths -from knowrob_ros.action import AskOneAction, AskOneGoal # Replace with actual import paths import actionlib +from knowrob_askone.msg import ( + KeyValuePair, + AskOneAction, + AskOneGoal, + GraphQueryMessage, + GraphAnswerMessage, +) class KnowRobRosLib: From 57218ccbb981fdd8975736bd0893eed7128cce87 Mon Sep 17 00:00:00 2001 From: Sascha Jongebloed Date: Thu, 24 Apr 2025 17:46:00 +0200 Subject: [PATCH 3/4] added rostest for askall and askone --- CMakeLists.txt | 5 ++ Dockerfile | 1 + action/AskAll.action | 2 +- msg/GraphQueryMessage.msg | 2 +- scripts/test_knowrob_ros_lib.py | 69 +++++++++++----- src/ROSInterface.cpp | 2 +- src/knowrob_ros_lib/knowrob_ros_lib.py | 109 +++++++++++++++++-------- 7 files changed, 129 insertions(+), 61 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 3ebd0aa..4b9dbde 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -82,6 +82,11 @@ file (COPY DESTINATION ${CATKIN_DEVEL_PREFIX}/lib/python3/dist-packages/knowrob_ros/ ) +catkin_install_python(PROGRAMS + scripts/test_knowrob_ros_lib.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + ## ============================= ## Install binaries ## ============================= diff --git a/Dockerfile b/Dockerfile index e13abd1..aee85c7 100755 --- a/Dockerfile +++ b/Dockerfile @@ -30,6 +30,7 @@ WORKDIR /catkin_ws RUN /usr/bin/catkin init RUN . /opt/ros/noetic/setup.sh && /usr/bin/catkin build +RUN echo "aaaa" # Build workspace with knowrob_ros WORKDIR /catkin_ws/src ADD . /catkin_ws/src/knowrob_ros diff --git a/action/AskAll.action b/action/AskAll.action index 7f15ba1..a0a3b04 100644 --- a/action/AskAll.action +++ b/action/AskAll.action @@ -4,7 +4,7 @@ byte FALSE = 0 byte TRUE = 1 byte QUERY_FAILED = 2 -GraphAnswerMessage[] answer +GraphAnswerMessage[] answers byte status --- uint32 numberOfSolutions \ No newline at end of file diff --git a/msg/GraphQueryMessage.msg b/msg/GraphQueryMessage.msg index 6f296bd..bf1cce1 100644 --- a/msg/GraphQueryMessage.msg +++ b/msg/GraphQueryMessage.msg @@ -1,5 +1,5 @@ # Languages in which graph queries can be encoded. -uint8 LANG_FOL=0 # Prolog-like syntax, @see .... +string LANG_FOL="LANG_FOL" # Prolog-like syntax, @see .... # Configure the language in which the graph query is encoded. string lang # Default: LANG_FOL diff --git a/scripts/test_knowrob_ros_lib.py b/scripts/test_knowrob_ros_lib.py index d4bfdf4..e438c13 100644 --- a/scripts/test_knowrob_ros_lib.py +++ b/scripts/test_knowrob_ros_lib.py @@ -1,25 +1,38 @@ -import knowrob_ros.knowrob_ros_lib +from knowrob_ros.knowrob_ros_lib import KnowRobRosLib, graph_answer_to_dict, get_default_modalframe, graph_answers_to_list import unittest -from knowrob_ros import AskOneAction, AskOneGoal # Replace with actual import paths - - +import rosunit +from knowrob_ros.msg import ( + KeyValuePair, + AskOneAction, + AskOneGoal, + AskOneResult, + AskAllAction, + AskAllGoal, + AskAllResult, + GraphQueryMessage, + GraphAnswerMessage, +) class TestKnowrobRosLib(unittest.TestCase): - # def test_ask_all(self): - # # Test the ask_all function - # result = knowrob_ros.ask_all("lpn:jelous(vincent, X)") - # self.assertEqual(result.bindings, [{ - # 'X': 'hans' - # }]) + def test_ask_all(self): + # Test the ask_one function + ask_all_result = self.knowrob_ros.ask_all("lpn:jealous(lpn:vincent, X)", get_default_modalframe()) + self.assertTrue(ask_all_result.status == AskAllResult.TRUE) + result_dict = graph_answers_to_list(ask_all_result.answers) + print("Result dict:", str(result_dict)) + self.assertEqual(result_dict, [{ + 'X': 'http://knowrob.org/kb/lpn#marsellus' + }]) def test_ask_one(self): # Test the ask_one function - ask_one_result = knowrob_ros.ask_one("lpn:jealous(lpn:vincent, X)") - self.assertTrue(ask_one_result.status == AskOneGoal.TRUE) - result = knowrob_ros_lib.graph_answer_to_dicts(ask_one_result.answer) - self.assertEqual(result.bindings, [{ - 'X': 'lpn:marsellus' - }]) + ask_one_result = self.knowrob_ros.ask_one("lpn:jealous(lpn:vincent, X)", get_default_modalframe()) + self.assertTrue(ask_one_result.status == AskOneResult.TRUE) + result_dict = graph_answer_to_dict(ask_one_result.answer) + print("Result dict:", str(result_dict)) + self.assertEqual(result_dict, { + 'X': 'http://knowrob.org/kb/lpn#marsellus' + }) # def test_tell(self): # # Create the triples to be added @@ -37,15 +50,27 @@ def test_ask_one(self): # }]) # Init the test class - def setUp(self): + @classmethod + def setUpClass(cls): # Initialize the knowrob_ros_lib - self.knowrob_ros = knowrob_ros_lib.KnowRobRosLib() + cls.knowrob_ros = KnowRobRosLib() # Initialize the ROS node - self.knowrob_ros.init_node("test_knowrob_ros_lib") + cls.knowrob_ros.init_node("test_knowrob_ros_lib") - def tearDown(self): + @classmethod + def tearDownClass(cls): # Shutdown the ROS node - self.knowrob_ros.shutdown_node() + cls.knowrob_ros.shutdown_node() + +@classmethod +def setUpClass(cls): + cls.knowrob_ros = KnowRobRosLib() + cls.knowrob_ros.init_node("test_knowrob_ros_lib") + if __name__ == '__main__': - unittest.main() \ No newline at end of file + rosunit.unitrun( + 'knowrob_ros', # your package + 'test_knowrob_ros_lib', # test name + TestKnowrobRosLib # your TestCase + ) \ No newline at end of file diff --git a/src/ROSInterface.cpp b/src/ROSInterface.cpp index 5355599..8b9d72a 100644 --- a/src/ROSInterface.cpp +++ b/src/ROSInterface.cpp @@ -155,7 +155,7 @@ void ROSInterface::executeAskAllCB(const AskAllGoalConstPtr &goal) { } else { // Push one answer GraphAnswerMessage graphAns = createGraphAnswer(positiveAnswer); - result.answer.push_back(graphAns); + result.answers.push_back(graphAns); numSolutions_ += 1; // publish feedback AskAllFeedback feedback; diff --git a/src/knowrob_ros_lib/knowrob_ros_lib.py b/src/knowrob_ros_lib/knowrob_ros_lib.py index 19f6943..114ae99 100644 --- a/src/knowrob_ros_lib/knowrob_ros_lib.py +++ b/src/knowrob_ros_lib/knowrob_ros_lib.py @@ -1,11 +1,16 @@ import rospy import actionlib -from knowrob_askone.msg import ( +from knowrob_ros.msg import ( KeyValuePair, AskOneAction, AskOneGoal, + AskOneResult, + AskAllAction, + AskAllGoal, + AskAllResult, GraphQueryMessage, GraphAnswerMessage, + ModalFrame ) @@ -15,21 +20,35 @@ def __init__(self): def init_node(self, name): rospy.init_node(name, anonymous=True) - self._ask_one_client = actionlib.SimpleActionClient("knowrob/ask_one", AskOneAction) + self._ask_one_client = actionlib.SimpleActionClient("knowrob/askone", AskOneAction) self._ask_one_client.wait_for_server() + self._ask_all_client = actionlib.SimpleActionClient("knowrob/askall", AskAllAction) + self._ask_all_client.wait_for_server() def shutdown_node(self): rospy.signal_shutdown("KnowRob node shutdown") if self._ask_one_client: self._ask_one_client.cancel_all_goals() - def ask_one(self, query): + def ask_one(self, query, modal_frame, lang=GraphQueryMessage.LANG_FOL): goal = AskOneGoal() - goal.query.query_string = query + goal.query.queryString = query + goal.query.frame = modal_frame + goal.query.lang = lang self._ask_one_client.send_goal(goal) self._ask_one_client.wait_for_result() result = self._ask_one_client.get_result() return result + + def ask_all(self, query, modal_frame, lang=GraphQueryMessage.LANG_FOL): + goal = AskAllGoal() + goal.query.queryString = query + goal.query.frame = modal_frame + goal.query.lang = lang + self._ask_all_client.send_goal(goal) + self._ask_all_client.wait_for_result() + result = self._ask_all_client.get_result() + return result # def tell(self, triples_str): # request = TellRequest() @@ -42,38 +61,56 @@ def ask_one(self, query): # # Replace with actual implementation for asking all # return GraphResultAdapter(GraphAnswerMessage(bindings=[KeyValuePair(key="X", value="hans")])) - def graph_answer_to_dicts(answer_msg): - """ - Convert a GraphAnswerMessage to a list of dictionaries. - Each dictionary represents one solution, where keys are variable names, - and values are decoded based on the type field. - """ - - results = [] - - for binding_group in answer_msg.bindings: - result = {} - for pair in binding_group.bindings: - if pair.type == KeyValuePair.TYPE_STRING: - result[pair.key] = pair.value_string - elif pair.type == KeyValuePair.TYPE_FLOAT: - result[pair.key] = pair.value_float - elif pair.type == KeyValuePair.TYPE_INT: - result[pair.key] = pair.value_int - elif pair.type == KeyValuePair.TYPE_LONG: - result[pair.key] = pair.value_long - elif pair.type == KeyValuePair.TYPE_VARIABLE: - result[pair.key] = pair.value_variable - elif pair.type == KeyValuePair.TYPE_PREDICATE: - result[pair.key] = pair.value_predicate - elif pair.type == KeyValuePair.TYPE_LIST: - # Lists are stored as a raw string and require custom parsing - result[pair.key] = pair.value_list - else: - result[pair.key] = None # Unknown type - results.append(result) - - return results +def get_default_modalframe(): + modalframe = ModalFrame() + modalframe.epistemicOperator = ModalFrame.KNOWLEDGE + modalframe.temporalOperator = ModalFrame.CURRENTLY + modalframe.minPastTimestamp = ModalFrame.UNSPECIFIED_TIMESTAMP + modalframe.maxPastTimestamp = ModalFrame.UNSPECIFIED_TIMESTAMP + modalframe.confidence = 0.0 + return modalframe + +def graph_answer_to_dict(answer_msg): + """ + Convert a GraphAnswerMessage to a dictionary format. + The dictionary will have the keys as the variable names and the values as the corresponding values. + """ + + results = {} + + for binding_group in answer_msg.substitution: + if binding_group.type == KeyValuePair.TYPE_STRING: + results[binding_group.key] = binding_group.value_string + elif binding_group.type == KeyValuePair.TYPE_FLOAT: + results[binding_group.key] = binding_group.value_float + elif binding_group.type == KeyValuePair.TYPE_INT: + results[binding_group.key] = binding_group.value_int + elif binding_group.type == KeyValuePair.TYPE_LONG: + results[binding_group.key] = binding_group.value_long + elif binding_group.type == KeyValuePair.TYPE_VARIABLE: + results[binding_group.key] = binding_group.value_variable + elif binding_group.type == KeyValuePair.TYPE_PREDICATE: + results[binding_group.key] = binding_group.value_predicate + elif binding_group.type == KeyValuePair.TYPE_LIST: + # Lists are stored as a raw string and require custom parsing + results[binding_group.key] = binding_group.value_list + else: + # Throw an error or handle unknown types + raise ValueError(f"Unknown type: {binding_group.type}") + + return results + +def graph_answers_to_list(answer_msgs): + """ + Convert a list of GraphAnswerMessage to a list of dictionaries. + Each dictionary will have the keys as the variable names and the values as the corresponding values. + """ + results = [] + for answer_msg in answer_msgs: + result = graph_answer_to_dict(answer_msg) + results.append(result) + return results + # class GraphResultAdapter: # def __init__(self, msg): From 7ce6651a6017a192d801b42ad1955590a5c8308b Mon Sep 17 00:00:00 2001 From: Sascha Jongebloed Date: Thu, 24 Apr 2025 17:51:16 +0200 Subject: [PATCH 4/4] updated readme --- README.md | 21 +++++++++++++-------- 1 file changed, 13 insertions(+), 8 deletions(-) diff --git a/README.md b/README.md index 8943be8..79d7655 100644 --- a/README.md +++ b/README.md @@ -74,18 +74,23 @@ Then run a query on another terminal: ```bash rostopic pub /knowrob/askone/goal knowrob_ros/AskOneActionGoal "header: seq: 0 - stamp: - secs: 0 - nsecs: 0 + stamp: {secs: 0, nsecs: 0} frame_id: '' goal_id: - stamp: - secs: 0 - nsecs: 0 + stamp: {secs: 0, nsecs: 0} id: '' goal: - query: {lang: '', queryString: 'lpn:jealous(lpn:vincent, X)', epistemicOperator: 0, aboutAgentIRI: '', aboutSimulationIRI: '', - temporalOperator: 0, minPastTimestamp: 0.0, maxPastTimestamp: 0.0, confidence: 0.0}" + query: + lang: '' + queryString: 'lpn:jealous(lpn:vincent, X)' + frame: + epistemicOperator: 0 + aboutAgentIRI: '' + aboutSimulationIRI: '' + temporalOperator: 0 + minPastTimestamp: 0.0 + maxPastTimestamp: 0.0 + confidence: 0.0" ``` You should now see an answer on the first terminal. \ No newline at end of file