diff --git a/CMakeLists.txt b/CMakeLists.txt index 67b5b97..565d552 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -24,6 +24,9 @@ add_message_files( DIRECTORY msg FILES KeyValuePair.msg + ModalFrame.msg + Triple.msg + TellMessage.msg GraphQueryMessage.msg GraphAnswerMessage.msg EventToken.msg diff --git a/Dockerfile b/Dockerfile index 2a18512..e13abd1 100755 --- a/Dockerfile +++ b/Dockerfile @@ -23,17 +23,19 @@ RUN apt install -y ros-noetic-tf2-geometry-msgs RUN mkdir /catkin_ws RUN mkdir /catkin_ws/src +# Build workspace with knowrob WORKDIR /catkin_ws/src -# Clone the knowrob repository RUN git clone https://github.com/knowrob/knowrob.git -# Add the knowrob-ros1 repository -ADD . /catkin_ws/src/knowrob_ros - WORKDIR /catkin_ws -# Build the catkin workspace RUN /usr/bin/catkin init RUN . /opt/ros/noetic/setup.sh && /usr/bin/catkin build +# Build workspace with knowrob_ros +WORKDIR /catkin_ws/src +ADD . /catkin_ws/src/knowrob_ros +WORKDIR /catkin_ws +RUN . /opt/ros/noetic/setup.sh && /usr/bin/catkin build + COPY run_knowrob.sh /run_knowrob.sh COPY run_knowrob_local.sh /run_knowrob_local.sh diff --git a/action/Tell.action b/action/Tell.action index fd52d2a..d1fdf4a 100644 --- a/action/Tell.action +++ b/action/Tell.action @@ -1,4 +1,4 @@ -GraphQueryMessage query +TellMessage tell --- byte TRUE = 0 byte TELL_FAILED = 1 diff --git a/include/ROSInterface.h b/include/ROSInterface.h index 229710b..5f5bbad 100644 --- a/include/ROSInterface.h +++ b/include/ROSInterface.h @@ -15,6 +15,7 @@ #include "knowrob/KnowledgeBase.h" #include "knowrob/queries/QueryParser.h" #include "knowrob/formulas/ModalFormula.h" +#include "knowrob/terms/String.h" // ROS #include #include @@ -28,7 +29,9 @@ #include #include #include +#include #include + // std #include @@ -114,7 +117,7 @@ namespace knowrob { * @param query A GraphQueryMessage * @return Map of key-value pairs */ - std::unordered_map translateGraphQueryMessage(const GraphQueryMessage &query); + std::unordered_map translateModalityFrameMessage(const ModalFrame &frame); /** * Translate a answer into a GraphAnswerMessage diff --git a/msg/GraphQueryMessage.msg b/msg/GraphQueryMessage.msg index ecf9f9f..6f296bd 100644 --- a/msg/GraphQueryMessage.msg +++ b/msg/GraphQueryMessage.msg @@ -1,21 +1,6 @@ # Languages in which graph queries can be encoded. uint8 LANG_FOL=0 # Prolog-like syntax, @see .... -# Epistemic operators distinguishing knowledge and belief. -# Note that the operators can be indexed by an agent -# meaning that another agent has the knowledge or belief. -uint8 KNOWLEDGE=0 # "it is certain that ..." -uint8 BELIEF=1 # "it could be that ..." - -# Temporal operators used to query truth -# of statements within time intervals. -uint8 CURRENTLY=0 # "it is now the case that ..." -uint8 ALL_PAST=1 # "it always was that ..." -uint8 SOME_PAST=2 # "it was the case that ..." - -# Used to indicate that a timestamp remains unspecified. -float64 UNSPECIFIED_TIMESTAMP=-1.0 - # Configure the language in which the graph query is encoded. string lang # Default: LANG_FOL @@ -26,42 +11,5 @@ string lang # Default: LANG_FOL # via the "lang" field. string queryString -# Determines the epistemic modality which is concerned -# with distinguishing knowledge (certain) from belief (uncertain). -# Defaults to KNOWLEDGE modality. -uint8 epistemicOperator # Default: KNOWLEDGE - -# The IRI of another agent (i.e. not the controlled agent). -# Used to query whether another agent might has some knowledge -# or belief. This is thought to be used in HRI and multirobot -# scenarios where the robot estimates knowledge and belief of -# another agent. -# Defaults to empty string indicating that the query is about -# the agent's own knowledge or belief. -# Cannot be set in parallel with the aboutSimulationIRI parameter -string aboutAgentIRI # Default: "" - -# The IRI to an KG of a mental simulation. -# The IRI will be returned as the output of an MentalSimulation -# Action. This is thought to be used in mental emulation -# and also visual anticipation tasks. -# Defaults to empty string indicating that the query is about -# the agen'ts own knowledge or beliefstate. -# Cannot be set in parallel with the aboutAgenIRI parameter -string aboutSimulationIRI # Default: "" - -# Determines the temporal modality of the query. -# Temporal modalities are used to cope with characteristics -# that change over time. -uint8 temporalOperator # Default: CURRENTLY - -# The time interval considered by the past operator can be constrained -# by min/max timestamp parameters. -# If minPastTimestamp>0, then statements are only considered that are -# true after the time indicated by minPastTimestamp. Similarly for maxPastTimestamp. -# It is also allowed to set only one of the fields to a positive value. -float64 minPastTimestamp # Default: UNSPECIFIED_TIMESTAMP -float64 maxPastTimestamp # Default: UNSPECIFIED_TIMESTAMP - -# The minimal confidence of the statement -float64 confidence # Default: 0.0 \ No newline at end of file +# Determines the modal frame in which the triple is assumed to be true. +ModalFrame frame diff --git a/msg/ModalFrame.msg b/msg/ModalFrame.msg new file mode 100644 index 0000000..8425dd4 --- /dev/null +++ b/msg/ModalFrame.msg @@ -0,0 +1,54 @@ +# Epistemic operators distinguishing knowledge and belief. +# Note that the operators can be indexed by an agent +# meaning that another agent has the knowledge or belief. +uint8 KNOWLEDGE=0 # "it is certain that ..." +uint8 BELIEF=1 # "it could be that ..." + +# Temporal operators used to query truth +# of statements within time intervals. +uint8 CURRENTLY=0 # "it is now the case that ..." +uint8 ALL_PAST=1 # "it always was that ..." +uint8 SOME_PAST=2 # "it was the case that ..." + +# Used to indicate that a timestamp remains unspecified. +float64 UNSPECIFIED_TIMESTAMP=-1.0 + +# Determines the epistemic modality which is concerned +# with distinguishing knowledge (certain) from belief (uncertain). +# Defaults to KNOWLEDGE modality. +uint8 epistemicOperator # Default: KNOWLEDGE + +# The IRI of another agent (i.e. not the controlled agent). +# Used to query whether another agent might has some knowledge +# or belief. This is thought to be used in HRI and multirobot +# scenarios where the robot estimates knowledge and belief of +# another agent. +# Defaults to empty string indicating that the query is about +# the agent's own knowledge or belief. +# Cannot be set in parallel with the aboutSimulationIRI parameter +string aboutAgentIRI # Default: "" + +# The IRI to an KG of a mental simulation. +# The IRI will be returned as the output of an MentalSimulation +# Action. This is thought to be used in mental emulation +# and also visual anticipation tasks. +# Defaults to empty string indicating that the query is about +# the agen'ts own knowledge or beliefstate. +# Cannot be set in parallel with the aboutAgenIRI parameter +string aboutSimulationIRI # Default: "" + +# Determines the temporal modality of the query. +# Temporal modalities are used to cope with characteristics +# that change over time. +uint8 temporalOperator # Default: CURRENTLY + +# The time interval considered by the past operator can be constrained +# by min/max timestamp parameters. +# If minPastTimestamp>0, then statements are only considered that are +# true after the time indicated by minPastTimestamp. Similarly for maxPastTimestamp. +# It is also allowed to set only one of the fields to a positive value. +float64 minPastTimestamp # Default: UNSPECIFIED_TIMESTAMP +float64 maxPastTimestamp # Default: UNSPECIFIED_TIMESTAMP + +# The minimal confidence of the statement +float64 confidence # Default: 0.0 diff --git a/msg/TellMessage.msg b/msg/TellMessage.msg new file mode 100644 index 0000000..21cf947 --- /dev/null +++ b/msg/TellMessage.msg @@ -0,0 +1,2 @@ +Triple[] triples +ModalFrame frame \ No newline at end of file diff --git a/msg/Triple.msg b/msg/Triple.msg new file mode 100644 index 0000000..e6fe728 --- /dev/null +++ b/msg/Triple.msg @@ -0,0 +1,3 @@ +string subject +string predicate +string object diff --git a/src/ROSInterface.cpp b/src/ROSInterface.cpp index db87eda..5355599 100644 --- a/src/ROSInterface.cpp +++ b/src/ROSInterface.cpp @@ -54,15 +54,15 @@ ROSInterface::ROSInterface(const boost::property_tree::ptree &config) ROSInterface::~ROSInterface() = default; // Function to convert GraphQueryMessage to std::unordered_map -std::unordered_map ROSInterface::translateGraphQueryMessage(const GraphQueryMessage &query) { +std::unordered_map ROSInterface::translateModalityFrameMessage(const ModalFrame &frame) { std::unordered_map options; - options["epistemicOperator"] = int(query.epistemicOperator); - options["aboutAgentIRI"] = query.aboutAgentIRI; - options["confidence"] = query.confidence; - options["temporalOperator"] = int(query.temporalOperator); - options["minPastTimestamp"] = query.minPastTimestamp; - options["maxPastTimestamp"] = query.maxPastTimestamp; + options["epistemicOperator"] = int(frame.epistemicOperator); + options["aboutAgentIRI"] = frame.aboutAgentIRI; + options["confidence"] = frame.confidence; + options["temporalOperator"] = int(frame.temporalOperator); + options["minPastTimestamp"] = frame.minPastTimestamp; + options["maxPastTimestamp"] = frame.maxPastTimestamp; return options; } @@ -130,10 +130,9 @@ GraphAnswerMessage ROSInterface::createGraphAnswer(std::shared_ptrquery.queryString)); - FormulaPtr mPhi = InterfaceUtils::applyModality(translateGraphQueryMessage(goal->query), phi); + FormulaPtr mPhi = InterfaceUtils::applyModality(translateModalityFrameMessage(goal->query.frame), phi); auto ctx = std::make_shared(QUERY_FLAG_ALL_SOLUTIONS); auto resultStream = kb_->submitQuery(mPhi, ctx); @@ -178,10 +177,9 @@ void ROSInterface::executeAskAllCB(const AskAllGoalConstPtr &goal) { void ROSInterface::executeAskIncrementalCB(const AskIncrementalGoalConstPtr &goal) { std::lock_guard lock(query_mutex_); - // Implement your action here FormulaPtr phi(QueryParser::parse(goal->query.queryString)); - FormulaPtr mPhi = InterfaceUtils::applyModality(translateGraphQueryMessage(goal->query), phi); + FormulaPtr mPhi = InterfaceUtils::applyModality(translateModalityFrameMessage(goal->query.frame), phi); auto ctx = std::make_shared(QUERY_FLAG_ALL_SOLUTIONS); auto resultStream = kb_->submitQuery(mPhi, ctx); @@ -275,9 +273,10 @@ bool ROSInterface::handleAskIncrementalFinish(AskIncrementalFinish::Request &req } void ROSInterface::executeAskOneCB(const AskOneGoalConstPtr &goal) { + FormulaPtr phi(QueryParser::parse(goal->query.queryString)); - FormulaPtr mPhi = InterfaceUtils::applyModality(translateGraphQueryMessage(goal->query), phi); + FormulaPtr mPhi = InterfaceUtils::applyModality(translateModalityFrameMessage(goal->query.frame), phi); auto ctx = std::make_shared(QUERY_FLAG_ALL_SOLUTIONS); auto resultStream = kb_->submitQuery(mPhi, ctx); @@ -307,9 +306,24 @@ void ROSInterface::executeAskOneCB(const AskOneGoalConstPtr &goal) { } void ROSInterface::executeTellCB(const TellGoalConstPtr &goal) { - FormulaPtr phi(QueryParser::parse(goal->query.queryString)); - FormulaPtr mPhi = InterfaceUtils::applyModality(translateGraphQueryMessage(goal->query), phi); + // Create a vector of Formulas + std::vector formulas; + // For each triple + for (const auto &triple: goal->tell.triples) { + // Create a vector of Terms for subject and object + std::vector terms; + // Add subject + terms.push_back(std::make_shared(triple.subject.data())); + // Add object + terms.push_back(std::make_shared(triple.object.data())); + // Add to formulas + formulas.push_back(std::make_shared(triple.predicate, terms)); + } + // Create conjunction of all formulas + FormulaPtr phi = std::make_shared(formulas); + + FormulaPtr mPhi = InterfaceUtils::applyModality(translateModalityFrameMessage(goal->tell.frame), phi); bool success = InterfaceUtils::assertStatements(kb_, {mPhi});