diff --git a/CMakeLists.txt b/CMakeLists.txt
index 565d552..4b9dbde 100755
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -19,7 +19,11 @@ find_package(catkin REQUIRED COMPONENTS
knowrob
)
find_package(spdlog REQUIRED)
+find_package(rostest REQUIRED)
+## =============================
+## ROS message generation
+## =============================
add_message_files(
DIRECTORY msg
FILES
@@ -29,7 +33,6 @@ add_message_files(
TellMessage.msg
GraphQueryMessage.msg
GraphAnswerMessage.msg
- EventToken.msg
)
add_action_files(
DIRECTORY action
@@ -50,7 +53,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 +71,35 @@ 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:
+file (COPY
+ ${CMAKE_CURRENT_SOURCE_DIR}/src/knowrob_ros_lib/knowrob_ros_lib.py
+ 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
+## =============================
+
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/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/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
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/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/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/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..e438c13
--- /dev/null
+++ b/scripts/test_knowrob_ros_lib.py
@@ -0,0 +1,76 @@
+from knowrob_ros.knowrob_ros_lib import KnowRobRosLib, graph_answer_to_dict, get_default_modalframe, graph_answers_to_list
+import unittest
+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_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 = 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
+ # 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
+ @classmethod
+ def setUpClass(cls):
+ # Initialize the knowrob_ros_lib
+ cls.knowrob_ros = KnowRobRosLib()
+ # Initialize the ROS node
+ cls.knowrob_ros.init_node("test_knowrob_ros_lib")
+
+ @classmethod
+ def tearDownClass(cls):
+ # Shutdown the ROS 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__':
+ 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/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/__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/knowrob_ros_lib.py b/src/knowrob_ros_lib/knowrob_ros_lib.py
new file mode 100644
index 0000000..114ae99
--- /dev/null
+++ b/src/knowrob_ros_lib/knowrob_ros_lib.py
@@ -0,0 +1,163 @@
+import rospy
+import actionlib
+from knowrob_ros.msg import (
+ KeyValuePair,
+ AskOneAction,
+ AskOneGoal,
+ AskOneResult,
+ AskAllAction,
+ AskAllGoal,
+ AskAllResult,
+ GraphQueryMessage,
+ GraphAnswerMessage,
+ ModalFrame
+)
+
+
+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/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, modal_frame, lang=GraphQueryMessage.LANG_FOL):
+ goal = AskOneGoal()
+ 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()
+ # 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 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):
+# 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 @@
+
+
+
+
+
+
+
+
+
+