From a28408aee94ad83c4324b34b1348936fec47ec3b Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Wed, 30 Jan 2019 11:47:51 +0100 Subject: [PATCH 1/6] Added cxx flags to rddlparser. Fixed typo in RDDLTaskParser + removed ippc2018 option which was overloading the cpu and memory --- rosplan_dependencies/CMakeLists.txt | 4 ++-- rosplan_knowledge_base/src/RDDLTaskParser.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/rosplan_dependencies/CMakeLists.txt b/rosplan_dependencies/CMakeLists.txt index c25c5b3be..fb5e0c85d 100644 --- a/rosplan_dependencies/CMakeLists.txt +++ b/rosplan_dependencies/CMakeLists.txt @@ -50,7 +50,7 @@ if (LBITS EQUAL 64) else() SET (RDDL_CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -m32") endif(LBITS EQUAL 64) -SET (RDDL_CMAKE_CXX_FLAGS "${RDDL_CMAKE_CXX_FLAGS} -g -Wall -W -Wno-sign-compare -Wno-deprecated -ansi -pedantic -Werror -std=c++11") +SET (RDDL_CMAKE_CXX_FLAGS "${RDDL_CMAKE_CXX_FLAGS} -g -Wall -W -Wno-sign-compare -Wno-deprecated -ansi -pedantic -Werror -std=c++11 -O3 -DNDEBUG -fomit-frame-pointer") # Lexer and parser find_package(BISON 3.0.4 REQUIRED) @@ -78,7 +78,7 @@ include_directories("rddl_parser") add_library(rddl_parser SHARED ${BISON_RDDL_PARSER_OUTPUTS} ${FLEX_RDDL_SCANNER_OUTPUTS} ${RDDL_PARSER_SOURCES}) add_dependencies(rddl_parser ${catkin_EXPORTED_TARGETS}) -SET_TARGET_PROPERTIES(rddl_parser PROPERTIES COMPILE_FLAGS "${RDDL_CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS}") +SET_TARGET_PROPERTIES(rddl_parser PROPERTIES COMPILE_FLAGS "${RDDL_CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS}" LINK_FLAGS "-g -O3") ############# ## Install ## diff --git a/rosplan_knowledge_base/src/RDDLTaskParser.cpp b/rosplan_knowledge_base/src/RDDLTaskParser.cpp index 8d3fc7d2f..ae8cd8041 100644 --- a/rosplan_knowledge_base/src/RDDLTaskParser.cpp +++ b/rosplan_knowledge_base/src/RDDLTaskParser.cpp @@ -28,11 +28,11 @@ namespace KCL_rosplan { domain_name = rddlTask->domainName; problem_name = rddlTask->name; - ROS_INFO("KCL: (%s) Pre-pocessing domain...", ros::this_node::getName().c_str()); + ROS_INFO("KCL: (%s) Pre-processing domain...", ros::this_node::getName().c_str()); uninstantiated_SACs = rddlTask->SACs; Instantiator instantiator(rddlTask); instantiator.instantiate(false); - Preprocessor preprocessor(rddlTask, true); + Preprocessor preprocessor(rddlTask, false); // ipc2018 to false preprocessor.preprocess(false); ROS_INFO("KCL: (%s) RDDL domain is ready!", ros::this_node::getName().c_str()); From 5d5d2d1c83a835a3f7041ee06c9bf3c60a45ef18 Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Wed, 30 Jan 2019 20:13:08 +0100 Subject: [PATCH 2/6] Added RDDL Enumerable type handling --- .../RDDLKnowledgeBase.h | 3 + .../src/RDDLKnowledgeBase.cpp | 149 ++++++++++++------ rosplan_knowledge_msgs/CMakeLists.txt | 1 + .../ProblemGeneration/RDDLProblemGenerator.h | 4 +- .../RDDLProblemGenerator.cpp | 39 +++-- 5 files changed, 133 insertions(+), 63 deletions(-) diff --git a/rosplan_knowledge_base/include/rosplan_knowledge_base/RDDLKnowledgeBase.h b/rosplan_knowledge_base/include/rosplan_knowledge_base/RDDLKnowledgeBase.h index 60b7c8856..80db8b0bd 100644 --- a/rosplan_knowledge_base/include/rosplan_knowledge_base/RDDLKnowledgeBase.h +++ b/rosplan_knowledge_base/include/rosplan_knowledge_base/RDDLKnowledgeBase.h @@ -18,6 +18,7 @@ #include "rosplan_knowledge_msgs/GetDomainOperatorService.h" #include "rosplan_knowledge_msgs/GetDomainOperatorDetailsService.h" #include "rosplan_knowledge_msgs/GetDomainPredicateDetailsService.h" +#include "rosplan_knowledge_msgs/GetEnumerableTypeService.h" #include "rosplan_knowledge_msgs/DomainFormula.h" #include "rosplan_knowledge_msgs/GetAttributeService.h" @@ -78,6 +79,7 @@ namespace KCL_rosplan { ros::ServiceServer _setRDDLMaxNonDefSrv; ros::ServiceServer _setImmediateRewardsSrv; ros::ServiceServer _reloadDomainStructureSrv; + ros::ServiceServer _getEnumtypesSrv; bool getRDDLParams(rosplan_knowledge_msgs::GetRDDLParams::Request &req, rosplan_knowledge_msgs::GetRDDLParams::Response &res); bool setRDDLDiscountFactor(rosplan_knowledge_msgs::SetFloat::Request &req, rosplan_knowledge_msgs::SetFloat::Response &res); @@ -85,6 +87,7 @@ namespace KCL_rosplan { bool setRDDLMAxNonDefActions(rosplan_knowledge_msgs::SetInt::Request &req, rosplan_knowledge_msgs::SetInt::Response &res); bool computeImmediateReward(rosplan_knowledge_msgs::GetRDDLImmediateReward::Request &req, rosplan_knowledge_msgs::GetRDDLImmediateReward::Response &res); bool reloadDomain(rosplan_knowledge_msgs::ReloadRDDLDomainProblem::Request &req, rosplan_knowledge_msgs::ReloadRDDLDomainProblem::Response &res); + bool getEnumTypes(rosplan_knowledge_msgs::GetEnumerableTypeService::Request &req, rosplan_knowledge_msgs::GetEnumerableTypeService::Response &res); void removeFact(const rosplan_knowledge_msgs::KnowledgeItem &msg) override; public: RDDLKnowledgeBase(ros::NodeHandle& n); diff --git a/rosplan_knowledge_base/src/RDDLKnowledgeBase.cpp b/rosplan_knowledge_base/src/RDDLKnowledgeBase.cpp index d9beebe20..95dc22067 100644 --- a/rosplan_knowledge_base/src/RDDLKnowledgeBase.cpp +++ b/rosplan_knowledge_base/src/RDDLKnowledgeBase.cpp @@ -14,7 +14,7 @@ namespace KCL_rosplan { /* get domain name */ bool RDDLKnowledgeBase::getDomainName(rosplan_knowledge_msgs::GetDomainNameService::Request &req, - rosplan_knowledge_msgs::GetDomainNameService::Response &res) { + rosplan_knowledge_msgs::GetDomainNameService::Response &res) { if (!domain_parser.domain_parsed) return false; res.domain_name = domain_parser.domain_name; return true; @@ -28,7 +28,7 @@ namespace KCL_rosplan { // Iterate over types std::set types; std::set super_types; - for (auto it = domain_parser.rddlTask->types.begin() ; it != domain_parser.rddlTask->types.end(); ++it) { + for (auto it = domain_parser.rddlTask->types.begin(); it != domain_parser.rddlTask->types.end(); ++it) { // FIXME should check if a type is a already supertype so we have disjunct sets? // FIXME filter out basic types? if (it->first == "int" or it->first == "real" or it->first == "bool" or it->first == "object") continue; @@ -41,17 +41,34 @@ namespace KCL_rosplan { return true; } - /* get domain predicates */ + + bool RDDLKnowledgeBase::getEnumTypes(rosplan_knowledge_msgs::GetEnumerableTypeService::Request &req, + rosplan_knowledge_msgs::GetEnumerableTypeService::Response &res) { + for (auto it = domain_parser.rddlTask->types.begin(); it != domain_parser.rddlTask->types.end(); ++it) { + if (it->first == req.type_name) { + if (it->second->objects.size() > 0 and it->second->objects[0]->name[0] == '@') { + for (auto oit = it->second->objects.begin(); oit != it->second->objects.end(); ++oit) + res.values.push_back((*oit)->name); + } + else ROS_ERROR("KCL: (%s) Type \"%s\" is not an enumerable.", ros::this_node::getName().c_str(), req.type_name.c_str()); + break; + } + } + return true; + } + + /* get domain predicates */ bool RDDLKnowledgeBase::getPredicates(rosplan_knowledge_msgs::GetDomainAttributeService::Request &req, rosplan_knowledge_msgs::GetDomainAttributeService::Response &res) { // FIXME I consider a predicate any state-fluent or non-fluent with a bool type // FIXME what about interm fluents and observ-fluents? Not sure if supported by the parser yet - for (auto it = domain_parser.rddlTask->variableDefinitions.begin() ; - it != domain_parser.rddlTask->variableDefinitions.end() ; ++it) { + for (auto it = domain_parser.rddlTask->variableDefinitions.begin(); + it != domain_parser.rddlTask->variableDefinitions.end(); ++it) { // Check if the parametrized variables (pVariables) are predicates if ((it->second->variableType != ParametrizedVariable::STATE_FLUENT and - it->second->variableType != ParametrizedVariable::NON_FLUENT) or it->second->valueType->name != "bool") { + it->second->variableType != ParametrizedVariable::NON_FLUENT) or + it->second->valueType->name != "bool") { continue; // Skip } // predicate name @@ -70,15 +87,18 @@ namespace KCL_rosplan { rosplan_knowledge_msgs::GetDomainAttributeService::Response &res) { // FIXME I consider a function any state-fluent or non-fluent with a numeric type (real/int) // FIXME what about interm fluents and observ-fluents? Not sure if supported by the parser - for (auto it = domain_parser.rddlTask->variableDefinitions.begin() ; - it != domain_parser.rddlTask->variableDefinitions.end() ; ++it) { + for (auto it = domain_parser.rddlTask->variableDefinitions.begin(); + it != domain_parser.rddlTask->variableDefinitions.end(); ++it) { // Check if the parametrized variables (pVariables) are functions if ((it->second->variableType != ParametrizedVariable::STATE_FLUENT and it->second->variableType != ParametrizedVariable::NON_FLUENT) or - (it->second->valueType->name != "int" and it->second->valueType->name != "real")) continue; // Skip + (it->second->valueType->name == + "bool") // Skip only boolean, as all the other fluents will be handled as functions + /*(it->second->valueType->name != "int" and it->second->valueType->name != "real")*/) + continue; // Skip - // function name + // function name rosplan_knowledge_msgs::DomainFormula formula; formula.name = it->second->variableName; @@ -92,7 +112,8 @@ namespace KCL_rosplan { /* get domain operators */ bool RDDLKnowledgeBase::getOperators(rosplan_knowledge_msgs::GetDomainOperatorService::Request &req, rosplan_knowledge_msgs::GetDomainOperatorService::Response &res) { - for (auto it = domain_parser.rddlTask->variableDefinitions.begin(); it != domain_parser.rddlTask->variableDefinitions.end(); ++it) { + for (auto it = domain_parser.rddlTask->variableDefinitions.begin(); + it != domain_parser.rddlTask->variableDefinitions.end(); ++it) { if (it->second->variableType != ParametrizedVariable::ACTION_FLUENT) continue; // Operator name @@ -118,29 +139,33 @@ namespace KCL_rosplan { bool RDDLKnowledgeBase::getOperatorDetails(rosplan_knowledge_msgs::GetDomainOperatorDetailsService::Request &req, rosplan_knowledge_msgs::GetDomainOperatorDetailsService::Response &res) { // FIXME maybe could precompute them? - for (auto it = domain_parser.rddlTask->variableDefinitions.begin(); it != domain_parser.rddlTask->variableDefinitions.end(); ++it) { - if ((it->second->variableType != ParametrizedVariable::ACTION_FLUENT ) or - (it->second->variableName != req.name)) continue; + for (auto it = domain_parser.rddlTask->variableDefinitions.begin(); + it != domain_parser.rddlTask->variableDefinitions.end(); ++it) { + if ((it->second->variableType != ParametrizedVariable::ACTION_FLUENT) or + (it->second->variableName != req.name)) + continue; // operator name - res.op.formula.name = it->second->variableName; + res.op.formula.name = it->second->variableName; // operator parameters res.op.formula.typed_parameters = getTypedParams(it->second->params); // Compute preconditions - PosNegDomainFormula prec = RDDLUtils::getOperatorPreconditions(res.op.formula, domain_parser.uninstantiated_SACs); + PosNegDomainFormula prec = RDDLUtils::getOperatorPreconditions(res.op.formula, + domain_parser.uninstantiated_SACs); res.op.at_start_simple_condition = prec.pos; res.op.at_start_neg_condition = prec.neg; // Compute effects - EffectDomainFormula eff = RDDLUtils::getOperatorEffects(res.op.formula, domain_parser.rddlTask->CPFDefinitions); + EffectDomainFormula eff = RDDLUtils::getOperatorEffects(res.op.formula, + domain_parser.rddlTask->CPFDefinitions); res.op.at_end_add_effects = eff.add; res.op.at_end_del_effects = eff.del; res.op.probabilistic_effects = eff.prob; // Compute assign effects res.op.at_end_assign_effects = RDDLUtils::getOperatorAssignEffects(res.op.formula, - domain_parser.rddlTask->CPFDefinitions); + domain_parser.rddlTask->CPFDefinitions); return true; } if (req.name == "exogenous") { @@ -148,7 +173,8 @@ namespace KCL_rosplan { res.op.formula.name = req.name; // Compute effects - EffectDomainFormula eff = RDDLUtils::getOperatorEffects(res.op.formula, domain_parser.rddlTask->CPFDefinitions); + EffectDomainFormula eff = RDDLUtils::getOperatorEffects(res.op.formula, + domain_parser.rddlTask->CPFDefinitions); res.op.at_end_add_effects = eff.add; res.op.at_end_del_effects = eff.del; res.op.probabilistic_effects = eff.prob; @@ -159,7 +185,7 @@ namespace KCL_rosplan { /* get domain predicate details */ bool RDDLKnowledgeBase::getPredicateDetails(rosplan_knowledge_msgs::GetDomainPredicateDetailsService::Request &req, - rosplan_knowledge_msgs::GetDomainPredicateDetailsService::Response &res) { + rosplan_knowledge_msgs::GetDomainPredicateDetailsService::Response &res) { auto it = domain_parser.rddlTask->variableDefinitions.find(req.name); if (it != domain_parser.rddlTask->variableDefinitions.end()) { res.predicate.name = it->second->variableName; @@ -168,7 +194,7 @@ namespace KCL_rosplan { res.predicate.typed_parameters = getTypedParams(it->second->params); // predicate is sensed - if(sensed_predicates.find(it->second->variableName) == sensed_predicates.end()) { + if (sensed_predicates.find(it->second->variableName) == sensed_predicates.end()) { sensed_predicates[it->second->variableName] = false; } res.is_sensed = sensed_predicates[it->second->variableName]; @@ -243,7 +269,8 @@ namespace KCL_rosplan { } /* Get object instances */ - std::map > RDDLKnowledgeBase::getInstances(const std::map& instance_objects) { + std::map > + RDDLKnowledgeBase::getInstances(const std::map &instance_objects) { map> instances; for (auto it = instance_objects.begin(); it != instance_objects.end(); ++it) { if (it->first == "false" or it->first == "true") continue; // Skip the "true" and "false" objects @@ -261,7 +288,8 @@ namespace KCL_rosplan { // FIXME is this right? // Add all the predicates/functions from pvariables whose initial value is true or are numeric and have default value - for (auto it = domain_parser.rddlTask->variableDefinitions.begin() ; it != domain_parser.rddlTask->variableDefinitions.end() ; ++it) { + for (auto it = domain_parser.rddlTask->variableDefinitions.begin(); + it != domain_parser.rddlTask->variableDefinitions.end(); ++it) { // Check if the parametrized variables (pVariables) are predicates bool isBool = it->second->valueType->name == "bool"; if ((it->second->variableType != ParametrizedVariable::STATE_FLUENT and @@ -275,40 +303,48 @@ namespace KCL_rosplan { } // Get state fluents from the initial state - for (auto it = domain_parser.rddlTask->stateFluents.begin(); it != domain_parser.rddlTask->stateFluents.end(); ++it) { - factsfuncs[(*it)->fullName] = fillKI(*it, (*it)->params, (*it)->initialValue); // This will override any default variable set + for (auto it = domain_parser.rddlTask->stateFluents.begin(); + it != domain_parser.rddlTask->stateFluents.end(); ++it) { + factsfuncs[(*it)->fullName] = fillKI(*it, (*it)->params, + (*it)->initialValue); // This will override any default variable set } // Get non-fluents from the initial state - for (auto it = domain_parser.rddlTask->nonFluents.begin(); it != domain_parser.rddlTask->nonFluents.end(); ++it) { - factsfuncs[(*it)->fullName] = fillKI(*it, (*it)->params, (*it)->initialValue); // This will override any default variable set + for (auto it = domain_parser.rddlTask->nonFluents.begin(); + it != domain_parser.rddlTask->nonFluents.end(); ++it) { + factsfuncs[(*it)->fullName] = fillKI(*it, (*it)->params, + (*it)->initialValue); // This will override any default variable set } // Iterate map to fill the right structures for (auto it = factsfuncs.begin(); it != factsfuncs.end(); ++it) { - if (it->second.knowledge_type == rosplan_knowledge_msgs::KnowledgeItem::FACT) model_facts.push_back(it->second); - else if (it->second.knowledge_type == rosplan_knowledge_msgs::KnowledgeItem::FUNCTION) model_functions.push_back(it->second); + if (it->second.knowledge_type == rosplan_knowledge_msgs::KnowledgeItem::FACT) + model_facts.push_back(it->second); + else if (it->second.knowledge_type == rosplan_knowledge_msgs::KnowledgeItem::FUNCTION) + model_functions.push_back(it->second); } } - rosplan_knowledge_msgs::KnowledgeItem RDDLKnowledgeBase::fillKI(const ParametrizedVariable* var, const std::vector ¶ms, double initialValue) { + rosplan_knowledge_msgs::KnowledgeItem + RDDLKnowledgeBase::fillKI(const ParametrizedVariable *var, const std::vector ¶ms, + double initialValue) { rosplan_knowledge_msgs::KnowledgeItem item; item.initial_time = ros::Time::now(); if (var->valueType->name == "bool") { // We have a fact item.knowledge_type = rosplan_knowledge_msgs::KnowledgeItem::FACT; item.is_negative = (initialValue == 0); - } - else { + } else { //assert(var->valueType->name == "real" or var->valueType->name == "int"); item.knowledge_type = rosplan_knowledge_msgs::KnowledgeItem::FUNCTION; item.is_negative = false; item.function_value = initialValue; } item.attribute_name = var->variableName; + item.instance_type = var->valueType->name; // Add type - for (size_t i = 0; i < params.size(); ++i) { + for (size_t i = 0; i < params.size(); ++i) { diagnostic_msgs::KeyValue param; param.key = domain_parser.rddlTask->variableDefinitions[var->variableName]->params[i]->name; // Get the variable name/id from the variabledefinitions structure size_t pos = param.key.find('?'); @@ -331,8 +367,7 @@ namespace KCL_rosplan { if (var->valueType->name == "bool") { // We have a fact item.knowledge_type = rosplan_knowledge_msgs::KnowledgeItem::FACT; item.is_negative = (var->initialValue == 0); - } - else { + } else { //assert(var->valueType->name == "real" or var->valueType->name == "int"); item.knowledge_type = rosplan_knowledge_msgs::KnowledgeItem::FUNCTION; item.is_negative = false; @@ -340,6 +375,7 @@ namespace KCL_rosplan { } item.attribute_name = var->variableName; + item.instance_type = var->valueType->name; item.values.resize(var->params.size()); } @@ -354,7 +390,8 @@ namespace KCL_rosplan { for (auto it = instance_names.begin(); it != instance_names.end(); ++it) { item.values[param_index].value = *it; - std::string aux = (ground_params.size()) ? ground_params + ", " + *it : *it; // If it's first, don't add comma + std::string aux = (ground_params.size()) ? ground_params + ", " + *it + : *it; // If it's first, don't add comma if (param_index == var->params.size() - 1) { factsfuncs[var->variableName + "(" + aux + ")"] = item; // If grounded all params, save it (push_back copies the object) @@ -362,8 +399,7 @@ namespace KCL_rosplan { fillKIAddAllGroundedParameters(var, factsfuncs, item, aux, param_index + 1); // If still params to ground ground them all } - } - else factsfuncs[var->variableName] = item; // Add the item without parameters + } else factsfuncs[var->variableName] = item; // Add the item without parameters } @@ -375,9 +411,10 @@ namespace KCL_rosplan { void RDDLKnowledgeBase::parseDomain(const std::string &domain_file_path, const std::string &problem_file_path) { domain_path_ = domain_file_path; - RDDLTask* t = domain_parser.parseTask(domain_file_path, problem_file_path); // The parser stores the task + RDDLTask *t = domain_parser.parseTask(domain_file_path, problem_file_path); // The parser stores the task if (t == nullptr) { - ROS_ERROR("KCL: (%s) There were syntax errors in the domain or instance file.", ros::this_node::getName().c_str()); + ROS_ERROR("KCL: (%s) There were syntax errors in the domain or instance file.", + ros::this_node::getName().c_str()); ros::shutdown(); } if (problem_file_path != "") addInitialState(); @@ -390,12 +427,20 @@ namespace KCL_rosplan { _nh.param("max_nondef_actions", _max_nondef_actions, 1); - _getParamsService = _nh.advertiseService("state/rddl_parameters", &KCL_rosplan::RDDLKnowledgeBase::getRDDLParams, this); - _setRDDLDiscountFactorSrv = _nh.advertiseService("state/set_rddl_discount_factor", &KCL_rosplan::RDDLKnowledgeBase::setRDDLDiscountFactor, this); - _setRDDLHorizonSrv = _nh.advertiseService("state/set_rddl_horizon", &KCL_rosplan::RDDLKnowledgeBase::setRDDLHorizon, this); - _setRDDLMaxNonDefSrv = _nh.advertiseService("state/set_rddl_max_nondef_actions", &KCL_rosplan::RDDLKnowledgeBase::setRDDLMAxNonDefActions, this); - _setImmediateRewardsSrv = _nh.advertiseService("state/get_immediate_reward", &KCL_rosplan::RDDLKnowledgeBase::computeImmediateReward, this); - _reloadDomainStructureSrv = _nh.advertiseService("reload_rddl_domain", &KCL_rosplan::RDDLKnowledgeBase::reloadDomain, this); + _getParamsService = _nh.advertiseService("state/rddl_parameters", + &KCL_rosplan::RDDLKnowledgeBase::getRDDLParams, this); + _setRDDLDiscountFactorSrv = _nh.advertiseService("state/set_rddl_discount_factor", + &KCL_rosplan::RDDLKnowledgeBase::setRDDLDiscountFactor, this); + _setRDDLHorizonSrv = _nh.advertiseService("state/set_rddl_horizon", + &KCL_rosplan::RDDLKnowledgeBase::setRDDLHorizon, this); + _setRDDLMaxNonDefSrv = _nh.advertiseService("state/set_rddl_max_nondef_actions", + &KCL_rosplan::RDDLKnowledgeBase::setRDDLMAxNonDefActions, this); + _setImmediateRewardsSrv = _nh.advertiseService("state/get_immediate_reward", + &KCL_rosplan::RDDLKnowledgeBase::computeImmediateReward, this); + _reloadDomainStructureSrv = _nh.advertiseService("reload_rddl_domain", + &KCL_rosplan::RDDLKnowledgeBase::reloadDomain, this); + _getEnumtypesSrv = _nh.advertiseService("domain/enumerable_type", + &KCL_rosplan::RDDLKnowledgeBase::getEnumTypes, this); } bool RDDLKnowledgeBase::getRDDLParams(rosplan_knowledge_msgs::GetRDDLParams::Request &req, @@ -427,16 +472,18 @@ namespace KCL_rosplan { return true; } - bool RDDLKnowledgeBase::reloadDomain(rosplan_knowledge_msgs::ReloadRDDLDomainProblem::Request &req, rosplan_knowledge_msgs::ReloadRDDLDomainProblem::Response &res) { - RDDLTask* t = domain_parser.parseTask(domain_path_, req.problem_path, true); // The parser stores the task + bool RDDLKnowledgeBase::reloadDomain(rosplan_knowledge_msgs::ReloadRDDLDomainProblem::Request &req, + rosplan_knowledge_msgs::ReloadRDDLDomainProblem::Response &res) { + RDDLTask *t = domain_parser.parseTask(domain_path_, req.problem_path, true); // The parser stores the task if (t == nullptr) { - ROS_ERROR("KCL: (%s) There were syntax errors in the domain or instance file.", ros::this_node::getName().c_str()); + ROS_ERROR("KCL: (%s) There were syntax errors in the domain or instance file.", + ros::this_node::getName().c_str()); res.success = false; - } - else res.success = true; + } else res.success = true; return true; } + bool RDDLKnowledgeBase::computeImmediateReward(rosplan_knowledge_msgs::GetRDDLImmediateReward::Request &req, rosplan_knowledge_msgs::GetRDDLImmediateReward::Response &res) { // Compute current state diff --git a/rosplan_knowledge_msgs/CMakeLists.txt b/rosplan_knowledge_msgs/CMakeLists.txt index 715d9ff5d..26cb8b056 100644 --- a/rosplan_knowledge_msgs/CMakeLists.txt +++ b/rosplan_knowledge_msgs/CMakeLists.txt @@ -50,6 +50,7 @@ add_service_files( GetRDDLParams.srv GetRDDLImmediateReward.srv ReloadRDDLDomainProblem.srv + GetEnumerableTypeService.srv ) ## Generate added messages and services with any dependencies listed here diff --git a/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/RDDLProblemGenerator.h b/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/RDDLProblemGenerator.h index 427ab4f8e..db1f10e8b 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/RDDLProblemGenerator.h +++ b/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/RDDLProblemGenerator.h @@ -9,6 +9,7 @@ #include "rosplan_knowledge_msgs/GetDomainOperatorDetailsService.h" #include "rosplan_knowledge_msgs/GetDomainOperatorService.h" #include "rosplan_knowledge_msgs/GetRDDLParams.h" +#include "rosplan_knowledge_msgs/GetEnumerableTypeService.h" #include "rosplan_knowledge_msgs/ReloadRDDLDomainProblem.h" namespace KCL_rosplan { @@ -33,7 +34,8 @@ namespace KCL_rosplan { std::string _domain_name; std::string _non_fluents_name; ros::NodeHandle _nh; - ros::ServiceClient reload_domain_; + ros::ServiceClient _reload_domain; + std::map> _enumeration_types; public: RDDLProblemGenerator(const std::string& kb); void generateProblemFile(std::string &problemPath) override; diff --git a/rosplan_planning_system/src/ProblemGeneration/RDDLProblemGenerator.cpp b/rosplan_planning_system/src/ProblemGeneration/RDDLProblemGenerator.cpp index c05d97ef0..00e7dcdae 100644 --- a/rosplan_planning_system/src/ProblemGeneration/RDDLProblemGenerator.cpp +++ b/rosplan_planning_system/src/ProblemGeneration/RDDLProblemGenerator.cpp @@ -17,7 +17,7 @@ namespace KCL_rosplan { } _domain_name = nameSrv.response.domain_name; _non_fluents_name = "nf_" + _domain_name + "__generate_instance"; - reload_domain_ = _nh.serviceClient(kb + "/reload_rddl_domain"); + _reload_domain = _nh.serviceClient(kb + "/reload_rddl_domain"); } @@ -33,8 +33,8 @@ namespace KCL_rosplan { pFile.close(); rosplan_knowledge_msgs::ReloadRDDLDomainProblem srv; srv.request.problem_path = problemPath; - if (not reload_domain_.call(srv) or not srv.response.success) { - ROS_ERROR("KCL: (RDDLProblemGenerator) Failed to call service %s.", reload_domain_.getService().c_str()); + if (not _reload_domain.call(srv) or not srv.response.success) { + ROS_ERROR("KCL: (RDDLProblemGenerator) Failed to call service %s.", _reload_domain.getService().c_str()); } } @@ -54,17 +54,30 @@ namespace KCL_rosplan { // Print objects pFile << "\tobjects {" << std::endl; + std::string srv_name = "/" + knowledge_base + "/domain/enumerable_type"; + ros::ServiceClient getEnumerableType = _nh.serviceClient(srv_name); + std::map> instances = getInstances(); for (auto it = instances.begin(); it != instances.end(); ++it) { if (it->second.size() > 0) { - pFile << "\t\t" << it->first << ": " << "{"; - bool comma = false; - for (auto inst_it = it->second.begin(); inst_it != it->second.end(); ++inst_it) { - if (comma) pFile << ", "; - else comma = true; - pFile << *inst_it; + if ((*it->second.begin())[0] == '@') { // If it starts with @ means it's an enumeration + rosplan_knowledge_msgs::GetEnumerableTypeService params; + params.request.type_name = it->first; + if (!getEnumerableType.call(params)) { + ROS_ERROR("KCL: (RDDLProblemGenerator) Failed to call service %s", srv_name.c_str()); + } + _enumeration_types[it->first] = params.response.values; + } + else { + pFile << "\t\t" << it->first << ": " << "{"; + bool comma = false; + for (auto inst_it = it->second.begin(); inst_it != it->second.end(); ++inst_it) { + if (comma) pFile << ", "; + else comma = true; + pFile << *inst_it; + } + pFile << "};" << std::endl; } - pFile << "};" << std::endl; } } pFile << "\t};" << std::endl; @@ -241,7 +254,11 @@ namespace KCL_rosplan { pFile << ((pfit->is_negative == 0)? "true" : "false"); } else { // FUNCTION - pFile << pfit->function_value; + auto enum_type = _enumeration_types.find(pfit->instance_type); + if (enum_type != _enumeration_types.end()) { + pFile << enum_type->second[pfit->function_value]; + } + else pFile << pfit->function_value; } pFile << ";" << std::endl; } From c547e29be8e792fff041ff1ec5d937049844d45f Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Wed, 30 Jan 2019 20:23:56 +0100 Subject: [PATCH 3/6] Added service --- rosplan_knowledge_msgs/srv/GetEnumerableTypeService.srv | 4 ++++ 1 file changed, 4 insertions(+) create mode 100644 rosplan_knowledge_msgs/srv/GetEnumerableTypeService.srv diff --git a/rosplan_knowledge_msgs/srv/GetEnumerableTypeService.srv b/rosplan_knowledge_msgs/srv/GetEnumerableTypeService.srv new file mode 100644 index 000000000..af7353431 --- /dev/null +++ b/rosplan_knowledge_msgs/srv/GetEnumerableTypeService.srv @@ -0,0 +1,4 @@ +# Get all values of the enumeration type with the name 'typeName'. +string type_name +--- +string[] values From 31f827427004d91ce1916a72474b3fcc715999d2 Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Thu, 31 Jan 2019 16:01:16 +0100 Subject: [PATCH 4/6] Added server to get type of fluents --- .../rosplan_knowledge_base/RDDLKnowledgeBase.h | 3 +++ rosplan_knowledge_base/src/RDDLKnowledgeBase.cpp | 14 ++++++++++++++ rosplan_knowledge_msgs/CMakeLists.txt | 1 + rosplan_knowledge_msgs/srv/GetRDDLFluentType.srv | 4 ++++ .../ProblemGeneration/RDDLProblemGenerator.h | 2 ++ .../src/ProblemGeneration/RDDLProblemGenerator.cpp | 14 +++++++++++--- 6 files changed, 35 insertions(+), 3 deletions(-) create mode 100644 rosplan_knowledge_msgs/srv/GetRDDLFluentType.srv diff --git a/rosplan_knowledge_base/include/rosplan_knowledge_base/RDDLKnowledgeBase.h b/rosplan_knowledge_base/include/rosplan_knowledge_base/RDDLKnowledgeBase.h index 80db8b0bd..5f50248c7 100644 --- a/rosplan_knowledge_base/include/rosplan_knowledge_base/RDDLKnowledgeBase.h +++ b/rosplan_knowledge_base/include/rosplan_knowledge_base/RDDLKnowledgeBase.h @@ -26,6 +26,7 @@ #include "rosplan_knowledge_msgs/GetMetricService.h" #include "rosplan_knowledge_msgs/KnowledgeItem.h" #include "rosplan_knowledge_msgs/GetRDDLParams.h" +#include "rosplan_knowledge_msgs/GetRDDLFluentType.h" #include "rosplan_knowledge_msgs/GetRDDLImmediateReward.h" #include "rosplan_knowledge_msgs/SetFloat.h" #include "rosplan_knowledge_msgs/SetInt.h" @@ -80,6 +81,7 @@ namespace KCL_rosplan { ros::ServiceServer _setImmediateRewardsSrv; ros::ServiceServer _reloadDomainStructureSrv; ros::ServiceServer _getEnumtypesSrv; + ros::ServiceServer _getFluentTypeSrv; bool getRDDLParams(rosplan_knowledge_msgs::GetRDDLParams::Request &req, rosplan_knowledge_msgs::GetRDDLParams::Response &res); bool setRDDLDiscountFactor(rosplan_knowledge_msgs::SetFloat::Request &req, rosplan_knowledge_msgs::SetFloat::Response &res); @@ -88,6 +90,7 @@ namespace KCL_rosplan { bool computeImmediateReward(rosplan_knowledge_msgs::GetRDDLImmediateReward::Request &req, rosplan_knowledge_msgs::GetRDDLImmediateReward::Response &res); bool reloadDomain(rosplan_knowledge_msgs::ReloadRDDLDomainProblem::Request &req, rosplan_knowledge_msgs::ReloadRDDLDomainProblem::Response &res); bool getEnumTypes(rosplan_knowledge_msgs::GetEnumerableTypeService::Request &req, rosplan_knowledge_msgs::GetEnumerableTypeService::Response &res); + bool getFluentType(rosplan_knowledge_msgs::GetRDDLFluentType::Request &req, rosplan_knowledge_msgs::GetRDDLFluentType::Response &res); void removeFact(const rosplan_knowledge_msgs::KnowledgeItem &msg) override; public: RDDLKnowledgeBase(ros::NodeHandle& n); diff --git a/rosplan_knowledge_base/src/RDDLKnowledgeBase.cpp b/rosplan_knowledge_base/src/RDDLKnowledgeBase.cpp index 95dc22067..c8f2b14f5 100644 --- a/rosplan_knowledge_base/src/RDDLKnowledgeBase.cpp +++ b/rosplan_knowledge_base/src/RDDLKnowledgeBase.cpp @@ -441,6 +441,8 @@ namespace KCL_rosplan { &KCL_rosplan::RDDLKnowledgeBase::reloadDomain, this); _getEnumtypesSrv = _nh.advertiseService("domain/enumerable_type", &KCL_rosplan::RDDLKnowledgeBase::getEnumTypes, this); + _getFluentTypeSrv = _nh.advertiseService("domain/fluent_type", + &KCL_rosplan::RDDLKnowledgeBase::getFluentType, this); } bool RDDLKnowledgeBase::getRDDLParams(rosplan_knowledge_msgs::GetRDDLParams::Request &req, @@ -557,4 +559,16 @@ namespace KCL_rosplan { } } } + + bool RDDLKnowledgeBase::getFluentType(rosplan_knowledge_msgs::GetRDDLFluentType::Request &req, + rosplan_knowledge_msgs::GetRDDLFluentType::Response &res) { + for (auto it = domain_parser.rddlTask->variableDefinitions.begin(); it != domain_parser.rddlTask->variableDefinitions.end(); ++it) { + if (it->first == req.fluent_name) { + res.type = it->second->valueType->name; + return true; + } + } + ROS_ERROR("KCL: (%s) getFluentType: Fluent %s was not found!", ros::this_node::getName().c_str(), req.fluent_name.c_str()); + return true; + } } diff --git a/rosplan_knowledge_msgs/CMakeLists.txt b/rosplan_knowledge_msgs/CMakeLists.txt index 26cb8b056..50c9fba3c 100644 --- a/rosplan_knowledge_msgs/CMakeLists.txt +++ b/rosplan_knowledge_msgs/CMakeLists.txt @@ -51,6 +51,7 @@ add_service_files( GetRDDLImmediateReward.srv ReloadRDDLDomainProblem.srv GetEnumerableTypeService.srv + GetRDDLFluentType.srv ) ## Generate added messages and services with any dependencies listed here diff --git a/rosplan_knowledge_msgs/srv/GetRDDLFluentType.srv b/rosplan_knowledge_msgs/srv/GetRDDLFluentType.srv new file mode 100644 index 000000000..7eae63d95 --- /dev/null +++ b/rosplan_knowledge_msgs/srv/GetRDDLFluentType.srv @@ -0,0 +1,4 @@ +# Get all values of the enumeration type with the name 'typeName'. +string fluent_name +--- +string type diff --git a/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/RDDLProblemGenerator.h b/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/RDDLProblemGenerator.h index db1f10e8b..a02342c53 100644 --- a/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/RDDLProblemGenerator.h +++ b/rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/RDDLProblemGenerator.h @@ -11,6 +11,7 @@ #include "rosplan_knowledge_msgs/GetRDDLParams.h" #include "rosplan_knowledge_msgs/GetEnumerableTypeService.h" #include "rosplan_knowledge_msgs/ReloadRDDLDomainProblem.h" +#include "rosplan_knowledge_msgs/GetRDDLFluentType.h" namespace KCL_rosplan { class RDDLProblemGenerator : public ProblemGenerator { @@ -35,6 +36,7 @@ namespace KCL_rosplan { std::string _non_fluents_name; ros::NodeHandle _nh; ros::ServiceClient _reload_domain; + ros::ServiceClient _get_fluent_type; std::map> _enumeration_types; public: RDDLProblemGenerator(const std::string& kb); diff --git a/rosplan_planning_system/src/ProblemGeneration/RDDLProblemGenerator.cpp b/rosplan_planning_system/src/ProblemGeneration/RDDLProblemGenerator.cpp index 00e7dcdae..b09519d41 100644 --- a/rosplan_planning_system/src/ProblemGeneration/RDDLProblemGenerator.cpp +++ b/rosplan_planning_system/src/ProblemGeneration/RDDLProblemGenerator.cpp @@ -18,6 +18,7 @@ namespace KCL_rosplan { _domain_name = nameSrv.response.domain_name; _non_fluents_name = "nf_" + _domain_name + "__generate_instance"; _reload_domain = _nh.serviceClient(kb + "/reload_rddl_domain"); + _get_fluent_type = _nh.serviceClient(kb + "/domain/fluent_type"); } @@ -254,7 +255,14 @@ namespace KCL_rosplan { pFile << ((pfit->is_negative == 0)? "true" : "false"); } else { // FUNCTION - auto enum_type = _enumeration_types.find(pfit->instance_type); + rosplan_knowledge_msgs::GetRDDLFluentType gft; + gft.request.fluent_name = pfit->attribute_name; + if (!_get_fluent_type.call(gft)) { + ROS_ERROR("KCL: (PDDLProblemGenerator) Failed to call service %s: %s", + _get_fluent_type.getService().c_str(), gft.request.fluent_name.c_str()); + return; + } + auto enum_type = _enumeration_types.find(gft.response.type); if (enum_type != _enumeration_types.end()) { pFile << enum_type->second[pfit->function_value]; } @@ -271,14 +279,14 @@ namespace KCL_rosplan { rosplan_knowledge_msgs::GetAttributeService attrSrv; attrSrv.request.predicate_name = *it; if (!getPropsClient.call(attrSrv)) { - ROS_ERROR("KCL: (PDDLProblemGenerator) Failed to call service %s: %s", + ROS_ERROR("KCL: (RDDLProblemGenerator) Failed to call service %s: %s", state_proposition_service.c_str(), attrSrv.request.predicate_name.c_str()); continue; } // If it was not a proposition, try functions if (attrSrv.response.attributes.size() == 0 and !getFuncsClient.call(attrSrv)) { - ROS_ERROR("KCL: (PDDLProblemGenerator) Failed to call service %s: %s", + ROS_ERROR("KCL: (RDDLProblemGenerator) Failed to call service %s: %s", state_proposition_service.c_str(), attrSrv.request.predicate_name.c_str()); continue; } From 821067ec19f57f08343e1fbe1ee1bd476d9330a5 Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Thu, 31 Jan 2019 16:01:35 +0100 Subject: [PATCH 5/6] Fixed extra space breaking the simpleplanparser in rddlsim interface --- .../src/PlannerInterface/RDDLSIMPlannerInterface.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/rosplan_planning_system/src/PlannerInterface/RDDLSIMPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/RDDLSIMPlannerInterface.cpp index 517cd6043..23602e6ad 100644 --- a/rosplan_planning_system/src/PlannerInterface/RDDLSIMPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/RDDLSIMPlannerInterface.cpp @@ -102,9 +102,11 @@ namespace KCL_rosplan { std::string action_name = (match[1].str().empty())? match[3].str(): match[1].str(); planner_output += std::to_string(action_idx) + ": (" + action_name; // Action name std::istringstream p(match[2].str()); // parameters + for (auto it = match.begin(); it != match.end(); ++it) std::cout << *it << std::endl; std::string param; while (getline(p, param, ',')) { - planner_output += " "+ param; + if (param[0] != ' ') planner_output += " "; + planner_output += param; } planner_output += ") [0.001]\n"; // Close parenthesis and add duration } From c4aa303681ea6d9ca18ef74a78917cb09b1b4807 Mon Sep 17 00:00:00 2001 From: Gerard Canal Date: Thu, 31 Jan 2019 16:06:42 +0100 Subject: [PATCH 6/6] Removed debug output --- .../src/PlannerInterface/RDDLSIMPlannerInterface.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/rosplan_planning_system/src/PlannerInterface/RDDLSIMPlannerInterface.cpp b/rosplan_planning_system/src/PlannerInterface/RDDLSIMPlannerInterface.cpp index 23602e6ad..671468887 100644 --- a/rosplan_planning_system/src/PlannerInterface/RDDLSIMPlannerInterface.cpp +++ b/rosplan_planning_system/src/PlannerInterface/RDDLSIMPlannerInterface.cpp @@ -102,7 +102,6 @@ namespace KCL_rosplan { std::string action_name = (match[1].str().empty())? match[3].str(): match[1].str(); planner_output += std::to_string(action_idx) + ": (" + action_name; // Action name std::istringstream p(match[2].str()); // parameters - for (auto it = match.begin(); it != match.end(); ++it) std::cout << *it << std::endl; std::string param; while (getline(p, param, ',')) { if (param[0] != ' ') planner_output += " ";