diff --git a/CMakeLists.txt b/CMakeLists.txt index 4e3f4fb..9cb8a1b 100755 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,6 +49,7 @@ add_service_files( DIRECTORY srv FILES AskIncrementalFinish.srv + ExportTriples.srv ) generate_messages(DEPENDENCIES std_msgs diff --git a/include/ROSInterface.h b/include/ROSInterface.h index 5f5bbad..22227d0 100644 --- a/include/ROSInterface.h +++ b/include/ROSInterface.h @@ -30,6 +30,7 @@ #include #include #include +#include #include // std @@ -50,6 +51,7 @@ namespace knowrob { // ROS Services ros::ServiceServer ask_incremental_finish_service_; + ros::ServiceServer export_server_; // KnowledgeBase KnowledgeBasePtr kb_; @@ -110,6 +112,15 @@ namespace knowrob { bool handleAskIncrementalFinish(AskIncrementalFinish::Request &req, AskIncrementalFinish::Response &res); + /** + * Export the knowledge base to a file + * @param req ExportTriples::Request + * @param res ExportTriples::Response + * @return true if the export was successful + */ + bool executeExportCB(ExportTriples::Request &req, + ExportTriples::Response &res); + /** * Translate a GraphQueryMessage into a map of key-value pairs * that can be used by applyModality diff --git a/src/ROSInterface.cpp b/src/ROSInterface.cpp index 8b9d72a..a5591aa 100644 --- a/src/ROSInterface.cpp +++ b/src/ROSInterface.cpp @@ -49,6 +49,7 @@ ROSInterface::ROSInterface(const boost::property_tree::ptree &config) tell_action_server_.start(); ask_incremental_finish_service_ = nh_.advertiseService("knowrob/askincremental_finish", &ROSInterface::handleAskIncrementalFinish, this); + export_server_ = nh_.advertiseService("knowrob/export", &ROSInterface::executeExportCB, this); } ROSInterface::~ROSInterface() = default; @@ -313,10 +314,17 @@ void ROSInterface::executeTellCB(const TellGoalConstPtr &goal) { 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 subject (always IRIAtom) (make stringview before) + terms.push_back(IRIAtom::Tabled(triple.subject.data())); + // Add object (can be IRIAtom, String or Numeric) + // If triple.object.data has no surrounding single quotes, add quotes + // create a stringview + std::string objectString = triple.object.data(); + if (objectString.front() != '\'' && objectString.back() != '\'') { + objectString = "'" + objectString + "'"; + } + TermPtr objectTerm = QueryParser::parseConstant(objectString); + terms.push_back(objectTerm); // Add to formulas formulas.push_back(std::make_shared(triple.predicate, terms)); } @@ -339,6 +347,13 @@ void ROSInterface::executeTellCB(const TellGoalConstPtr &goal) { tell_action_server_.setSucceeded(result); } +bool ROSInterface::executeExportCB(ExportTriples::Request &req, + ExportTriples::Response &res) { + kb_->exportTo(req.path); + res.success = true; + return true; +} + int main(int argc, char **argv) { InitKnowRob(argc, argv); diff --git a/srv/ExportTriples.srv b/srv/ExportTriples.srv new file mode 100644 index 0000000..081c2c1 --- /dev/null +++ b/srv/ExportTriples.srv @@ -0,0 +1,4 @@ +string path +--- +# The result of the query +bool success \ No newline at end of file