RDDL fixes and improvements

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 ##

rosplan_knowledge_base/include/rosplan_knowledge_base/RDDLKnowledgeBase.h

@@ -18,13 +18,15 @@
 #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"
 #include "rosplan_knowledge_msgs/GetInstanceService.h"
 #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"
@@ -78,13 +80,17 @@ namespace KCL_rosplan {
 		ros::ServiceServer _setRDDLMaxNonDefSrv;
 		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);
 		bool setRDDLHorizon(rosplan_knowledge_msgs::SetInt::Request &req, rosplan_knowledge_msgs::SetInt::Response &res);
 		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);
+		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);

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());
 

rosplan_knowledge_msgs/CMakeLists.txt

@@ -50,6 +50,8 @@ add_service_files(
   GetRDDLParams.srv
   GetRDDLImmediateReward.srv
   ReloadRDDLDomainProblem.srv
+  GetEnumerableTypeService.srv
+  GetRDDLFluentType.srv
 )
 
 ## Generate added messages and services with any dependencies listed here

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

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

rosplan_planning_system/include/rosplan_planning_system/ProblemGeneration/RDDLProblemGenerator.h

@@ -9,7 +9,9 @@
 #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"
+#include "rosplan_knowledge_msgs/GetRDDLFluentType.h"
 
 namespace KCL_rosplan {
     class RDDLProblemGenerator : public ProblemGenerator {
@@ -33,7 +35,9 @@ namespace KCL_rosplan {
         std::string _domain_name;
         std::string _non_fluents_name;
         ros::NodeHandle _nh;
-        ros::ServiceClient reload_domain_;
+        ros::ServiceClient _reload_domain;
+        ros::ServiceClient _get_fluent_type;
+        std::map<std::string, std::vector<std::string>> _enumeration_types;
     public:
         RDDLProblemGenerator(const std::string& kb);
         void generateProblemFile(std::string &problemPath) override;

rosplan_planning_system/src/PlannerInterface/RDDLSIMPlannerInterface.cpp

@@ -104,7 +104,8 @@ namespace KCL_rosplan {
                         std::istringstream p(match[2].str()); // parameters
                         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
                     }

rosplan_planning_system/src/ProblemGeneration/RDDLProblemGenerator.cpp

@@ -17,7 +17,8 @@ namespace KCL_rosplan {
         }
         _domain_name = nameSrv.response.domain_name;
         _non_fluents_name = "nf_" + _domain_name + "__generate_instance";
-        reload_domain_ = _nh.serviceClient<rosplan_knowledge_msgs::ReloadRDDLDomainProblem>(kb + "/reload_rddl_domain");
+        _reload_domain = _nh.serviceClient<rosplan_knowledge_msgs::ReloadRDDLDomainProblem>(kb + "/reload_rddl_domain");
+        _get_fluent_type = _nh.serviceClient<rosplan_knowledge_msgs::GetRDDLFluentType>(kb + "/domain/fluent_type");
     }
 
 
@@ -33,8 +34,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 +55,30 @@ namespace KCL_rosplan {
        // Print objects
         pFile << "\tobjects {" << std::endl;
 
+        std::string srv_name = "/" + knowledge_base + "/domain/enumerable_type";
+        ros::ServiceClient getEnumerableType = _nh.serviceClient<rosplan_knowledge_msgs::GetEnumerableTypeService>(srv_name);
+
         std::map<std::string, std::vector<std::string>> 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 +255,18 @@ namespace KCL_rosplan {
                 pFile << ((pfit->is_negative == 0)? "true" : "false");
             }
             else { // FUNCTION
-                pFile << pfit->function_value;
+                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];
+                }
+                else pFile << pfit->function_value;
             }
             pFile << ";" << std::endl;
         }
@@ -254,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;
             }