Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@ add_service_files(
DIRECTORY srv
FILES
AskIncrementalFinish.srv
ExportTriples.srv
)
generate_messages(DEPENDENCIES
std_msgs
Expand Down
11 changes: 11 additions & 0 deletions include/ROSInterface.h
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@
#include <knowrob_ros/AskIncrementalFinish.h>
#include <knowrob_ros/TellAction.h>
#include <knowrob_ros/ModalFrame.h>
#include <knowrob_ros/ExportTriples.h>
#include <actionlib/server/simple_action_server.h>

// std
Expand All @@ -50,6 +51,7 @@ namespace knowrob {

// ROS Services
ros::ServiceServer ask_incremental_finish_service_;
ros::ServiceServer export_server_;

// KnowledgeBase
KnowledgeBasePtr kb_;
Expand Down Expand Up @@ -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
Expand Down
23 changes: 19 additions & 4 deletions src/ROSInterface.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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<TermPtr> terms;
// Add subject
terms.push_back(std::make_shared<String>(triple.subject.data()));
// Add object
terms.push_back(std::make_shared<String>(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<Predicate>(triple.predicate, terms));
}
Expand All @@ -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);

Expand Down
4 changes: 4 additions & 0 deletions srv/ExportTriples.srv
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
string path
---
# The result of the query
bool success